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I.  INTRODUCTION 


A.  GOALS 

The  goal  of  this  thesis  is  to  investigate  a  method  for 
producing  a  graphic  simulation  of  a  walking  robot  constructed 
from  serial  manipulators  acting  as  legs.  The  main  intent  is 
to  compare  object-oriented  code  that  is  based  on  kinematics 
using  two  programming  languages,  CLOS  and  C++.  This  thesis 
discusses  and  provides  examples  of  steps  necessary  for  the 
evolution  of  a  first  stage  graphic  simulator  of  a  walking 
robot.  The  walking  robot  in  question  is  a  six-legged 
underwater  vehicle,  called  "Aquarobot" ,  that  is  presently 
under  development  in  Japan  for  use  in  subsea  construction  and 
inspection  tasks. 

B.  ORGANIZATION 

Chapter  II  of  this  thesis  reviews  previous  work  in  the 
area  of  walking  robots.  Chapter  III  provides  a  detailed 
description  of  Aquarobot,  the  subject  of  the  simulator 
developed  in  this  research.  Chapter  IV  provides  an  overview 
of  kinematics  modelling  of  articulated  rigid  bodies,  and 
methods  used  to  calculate  link  parameters  for  such  systems. 
The  last  part  of  this  chapter  provides  the  specific  kinematic 
parameters  for  Aquarobot. 
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Chapter  V  is  a  review  of  object-oriented  programming  and 
includes  a  discussion  of  its  advantages  and  disadvantages. 
Chapter  VI  contains  the  history  and  a  description  of  some 
common  object-oriented  languages.  Chapter  VII  provides  a 
description  of  the  Aquarobot  simulation  programs  written  in 
the  CLOS  and  C++  languages.  This  chapter  compares  the  methods 
each  language  requires  to  define  classes  and  create  objects. 
A  comparison  of  the  performance  of  the  C++  and  CLOS 
simulations  is  provided  in  Chapter  VIII. 

The  last  chapter.  Chapter  IX,  presents  some  conclusions 
about  the  work  described.  This  is  followed  by  recommendations 
for  possible  future  use  of  Aquarobot,  the  characteristics  of 
the  two  simulations  created,  and  suggestions  for  further 
r asearch. 
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II.  8URVEY  OP  PREVIOUS  WORK 

A.  INTRODUCTION 

Man's  need  comprehend  the  human  body  and  the  phenomena 
around  hi™  motivates  him  to  imitate  it  as  a  tool  of 
understanding.  This  chapter  provides  a  historical  review  of 
robotic  advancements  in  living  animal  imitation.  It 
specifically  addresses  the  evolution  of  legged  robots.  The 
differences  between  legged  and  wheeled  1 'comotion  are  also 
discussed. 

B.  HISTORICAL  IMITATION  OP  LIVING  CREATURES 

Historically,  research  has  attempted  to  build  machines 
that  imitate  animals.  Through  technology,  it  is  hoped  to 
achieve  a  better  understanding  of  humans  and  animals  and  to 
accomplish  these  creature's  tasks  with  robots.  Some  such 
research  is  driven  by  a  desire  to  provide  the  disabled  with 
alternative  compensation  methods,  such  as  artificial  limbs, 
and  other  means  of  achieving  increased  mobility  (McGhee, 
1977) .  Mobility  goals  for  legged  vehicles  include  moving 
faster  or  for  extended  times,  or  operating  in  adverse 
environments  and  conditions  such  as  moving  under  water,  and  in 
space  flight  applications. 

Biological  systems,  often  taken  for  granted,  are  extremely 
difficult  to  emulate  or  even  define.  One  example,  the 
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imitation  of  a  walking  gait  of  an  animal,  is  not  easy  due  to 
the  difficulty  of  emulating  the  nervous  system  and  the  natural 
materials  that  form  the  animal.  These  unknown  variables  have 
impeded  our  success  in  obtaining  the  coordination  algorithms 
of  even  simple  animals  (McGhee,  1985).  A  human  takes 
approximately  one  year  to  learn  how  to  walk  yet,  after  decades 
of  research,  walking  machines  are  still  considered  to  be  in 
the  "infant"  stage. 

Animal  limb  imitation  has  been  an  area  of  great  interest 
to  researchers  interested  in  advanced  mobility  systems.  If  an 
application  for  a  walking  vehicle  is  known,  there  are  many 
variables  that  must  be  considered  to  determine  an  animal  to 
imitate.  As  an  example,  one  variable  is  compliance  (Anon, 
1987)  .  Compliance  is  defined  as  "the  act  of  conforming, 
acquiescing,  or  yielding"  (Stein,  1979)  .  As  the  degree  of 
compliance  of  a  design  is  improved,  the  machine  becomes  more 
challenging  to  control  and  keep  the  limb  steady,  yet  it  will 
be  more  robust  (e.g.,  able  to  withstand  impact).  If  the 
degree  of  compliance  in  a  design  is  reduced,  then  the  ability 
to  accurately  position  the  limb  will  be  enhanced,  but  it  will 
tend  to  be  rigid  and  unyielding.  On  the  other  hand, 
compliance  permits  flexibility  which  is  beneficial  when 
performing  simple  but  complex  actions  such  as  attempting  to 
place  a  bolt  on  a  screw  (Anon,  1987). 

Limb  imitation  designs  have  varied  drastically  in 
appearance.  For  access  to  tight  spaces,  snake-like  devices 
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have  been  constructed.  Their  applications  require  that 
compliance  be  limited  in  order  to  maintain  position.  In 
contrast,  a  limb  similar  to  an  elephant's  trunk  has  been  used 
as  a  device  to  lift  objects  of  varied  shapes.  This  device  did 
not  have  an  internal  support  structure.  Instead,  it  copied  the 
multiple  layers  of  muscle  in  an  elephant's  trunk  which 
provides  motion  control.  It  was  extremely  compliant  in  order 
to  accommodate  the  varied  shapes  that  grasped  objects  require 
(Anon,  1987) .  Human  hands  have  been  imitated  in  numerous 
designs.  Additionally,  legs  are  very  popular  in  robot 
research. 

Legged  locomotion  requires  a  successful  leg  design. 
Through  evolution,  animals  have  perfected  their  individual 
legged  locomotion  characteristics  based  on  their  specialized 
needs.  Legged  animals  are  capable  of  high  speeds  and 
intricate  motion  even  when  the  animal  is  large  and  heavy. 
Animal  legs  have  been  put  into  two  categories:  "mammal"  and 
"insect"  types  (Iw»saki,  1987).  The  "mammal"  type  has  legs 
which  are  always  vertical  like  a  horse.  The  "insect"  type  has 
bent  legs  like  a  beetle.  A  walking  capability  able  to 
function  in  natural  terrain  requires  complicated  sensors,  a 
nervous  system,  and  artificial  intelligence  (e.g.,  a  reasoning 
ability) .  Since  exact  imitation  of  these  intricate  animal 
systems  has  not,  at  present,  been  achieved,  legged  vehicle 
designers  must  choose  other  mean.:  to  solve  this  coordination 
control  problem  (McGhee,  1985) . 
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C.  HISTORY  G/  WALKING  ROBOTS 

The  original  legged  machines  evolved  from  earth  moving  and 
construction  vehicles.  These  devices  are  known  as  "climbing 
hoes"  (McGhee,  1985).  From  1965  to  1968,  a  four-legged 
vehicle,  called  the  "Quadruped  Transporter",  was  constructed 
by  General  Electric.  This  vehicle  incorporated  a  human 
operator  in  order  to  provide  the  sensing  and  neural  control 
functions  discussed  earlier.  The  operator  of  this  vehicle  was 
provided  with  one  leg  control  lever  for  each  limb.  These 
levers  were  attached  to  the  arms  and  legs  of  the  human 
operator  so  that  he  could  control  the  legs  by  executing  the 
desired  motions  with  his  own  limbs.  The  front  legs  were 
controlled  by  the  operator's  hands  and  the  rear  legs  were 
controlled  by  the  operator's  feet.  Each  control  lever  had 
three  degrees  of  freedom:  two  at  the  hip  and  one  at  the  knee. 
This  coordination  control  system  required  a  high  level  of 
operator  skill,  and  only  a  few  mastered  its  intricacies. 
Moreover,  these  operators  could  only  walk  the  vehicle  for  a 
short  time  due  to  the  complicated  multi-degree  of  freedom 
coordination  problem  (McGhee,  1985). 

The  Quadruped  Transporter  was  designed  as  a  research 
vehicle  and  opened  the  field  of  vehicular  legged  locomotion. 
A  hydraulic  servo  system  moved  the  legs.  It  successfully 
walked  and  displayed  impressive  obstacle  climbing  ability. 
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However,  the  complexity  of  the  operator  motion  coordination 
task  severely  limited  the  device's  capabilities  (McGhee, 
1985)  . 

In  1977,  a  different  control  method  was  incorporated  into 
another  robot  called  the  Ohio  State  University  (OSU)  Hexapod 
Vehicle.  This  robot  used  supervisory  control  (Ferrell,  1967) 
in  which  the  operator  controlled  speed  and  direction,  and  a 
computer  coordinated  the  actual  leg  motion  (Pugh,  1982) .  The 
OSU  Hexapod  Vehicle  was  a  six-legged  vehicle  with  insect  type 
legs  (McGhee,  1985) .  The  device  was  constructed  to  study  and 
develop  gait  algorithms.  Each  leg  had  three  degrees  of 
freedom,  each  consisting  of  two  links  connected  by  a  joint. 
Each  joint  had  an  electric  motor  and  a  worm  gear  (Waldron, 
1989)  .  The  operator  controlled  the  vehicle  with  a  remote 
joystick  in  an  indoor  laboratory  setting. 

The  successor  to  the  OSU  Hexapod  Vehicle  was  completed  in 
1986  at  OSU.  It  was  called  the  "Adaptive  Suspension  Vehicle" 
(ASV)  (Waldron,  1986) .  The  ASV  was  designed  for  sustained 
outdoor  locomotion  on  uneven  and  unmapped  terrain.  This  six¬ 
legged  robot  was  the  first  robot  to  control  its  legs  by  an  on¬ 
board  computer  and  to  carry  its  own  power  source  in  the  form 
of  an  internal  combustion  engine  (Waldron,  1986)  .  The  ASV, 
like  the  Quadruped  Transformer,  includes  an  onboard  human 
operator.  However,  the  ASV  does  not  require  manual 
coordination  of  limb  motion  by  the  operator  (Waldron,  1986) . 
In  order  for  the  ASV  to  operate  in  unstructured  terrain,  it 
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incorporates  extensive  sensor  devices  including  a  laser 
terrain  scanner  to  provide  a  three  dimensional  terrain 
elevation  map  for  a  distance  of  ten  meters  in  front  of  the 
vehicle.  This  information  is  used  for  automatic  selection  of 
footholds  in  rough  terrain  (Waldron,  1986) . 

To  date,  legged  vehicles  have  had  limited  application 
success.  This  is  due  to  the  complex  leg  coordination  control 
problem  and  a  limited  understanding  of  necessary  gait 
algorithms.  Also,  this  situation  exists  because  of  limited 
advances  in  leg  design.  Future  improvements  in  agility  and 
speed  are  anticipated  with  further  progress  in  understanding 
of  the  difficult  problem  of  microcomputer  coordination  of 
joint  motion  (McGhee,  1985) . 

D.  ADVANTAGES  OF  LEGGED  ROBOTS 

Legged  locomotion  has  existed  for  hundreds  of  millions  of 
years  while  wheeled  locomotion,  an  invention  of  man,  has  been 
around  for  only  several  thousand  years  (Waldron,  1989)  .  It  is 
interesting  that  evolution  has  not  produced  wheeled  biological 
systems,  but  then  there  were  no  smooth,  graded  roads  before 
the  introduction  of  wheels.  Still,  given  the  elegant  results 
of  evolution,  one  might  conclude  legged  locomotion  is 
inherently  superior  to  wheeled  locomotion,  at  least  in  natural 
terrain. 

Currently,  it  is  possible  to  go  close  to  most  places  of 
interest  on  the  land  surface  of  the  earth  by  traveling  on 
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roads.  This  has  greatly  altered  our  environment.  Still,  it 
takes  an  off-road  wheeled  or  tracked  vehicle  to  reach  the 
areas  in  between,  and  they  leave  ugly  ruts  in  the  soil.  If 
the  off -road  vehicle  were  to  be  a  legged  vehicle,  it  would 
leave  only  discrete  footprints.  Furthermore,  over  half  the 
Earth's  land  surface  (largely,  unpopulated  areas)  is  entirely 
inaccessible  to  wheeled  vehicles  (Waldron,  1989)  but  not  to 
legged  vehicles.  Legged  vehicles  have  the  potential  to  walk 
underwater  and  in  surf  as  well. 

Legged  locomotion  has  an  advantage  over  wheeled  locomotion 
when  soft  ground  or  slippery  surfaces  are  involved.  Wheeled 
vehicles  sink  into  the  ground  and  must  roll  out  of  the 
resulting  depression  by  relying  on  shearing  forces  resulting 
from  friction  between  wheels  and  the  ground.  Legs  also  sink 
into  the  ground  but  can  be  lifted  vertically  (Bekker,  1969)  - 
a  maneuver  that  doesn't  impede  locomotion. 

While  wheeled  vehicles  have  proven  themselves  efficient 
for  long-distance  transportation,  the  path  must  be  relatively 
smooth  and  firm.  The  performance  of  large  mammals  shows  that 
it  is  possible  for  legged  locomotion  to  also  be  efficient  for 
long-distance  transportation.  However,  actively  coordinated 
leg  motions  must  be  defined  by  algorithms.  These  algorithms 
are  presently  in  an  early  stage  of  development  (Waldron, 
1989)  . 

Legged  vehicles  may  eventually  be  able  to  compete  with 
wheeled  locomotion  in  all  respects  except  possibly  speed. 
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However,  additional  technological  advances  in  theory  and 
materials  will  be  needed  before  such  machines  can  reach  their 
full  potential.  The  advances  in  computers  in  the  late  1980's 
enabled  researchers  to  provide  for  the  leg  coordination 
computations  on  board  a  walking  vehicle,  but  researchers  are 
moving  slowly  in  their  attempts  to  provide  sufficiently 
powerful  computation  algorithms  (McGhee,  1985) .  Over  adverse 
terrain,  legged  vehicles  have  the  potential  to  provide  higher 
speed,  greater  mobility,  and  less  environmental  damage. 
Additionally,  legged  vehicles  can  provide  more  comfort  for  a 
human  rider.  The  rough  ride  wheeled  provided  by  locomotion 
over  rough  terrain  is  detrimental  to  instruments  and  cargo  on 
board.  In  contrast,  legged  vehicles  do  not  vibrate  when 
travelling  over  rough  terrain  (Waldron,  1989).  Finally, 
several  studies  have  shown  that  legged  vehicles  have  the 
potential  to  provide  improved  fuel  economy  in  comparison  with 
wheeled  vehicles  of  comparable  size  (McGhee,  1986) . 

E.  SUMMARY 

This  chapter  provides  a  survey  of  previous  work  relating 
to  and  walking  machines.  It  specifically  discusses  the 
history  of  legged  vehicle  technology  and  provides  walking 
machine  examples.  Legged  and  wheeled  locomotion  are  compared 
and  their  specific  advantages  are  discussed.  The  next  chapter 
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discusses  a  walking  robot,  Aquarobot,  that  is  currently  under 
development  in  Japan,  and  which  provides  the  focus  of  this 
thesis. 


III.  AQUAROBOT 


A.  INTRODUCTION 

One  of  Japan 's  most  important  resources  is  its  land. 
Unfortunately,  Japanese  tidal  waves  (tsunamis) ,  constantly 
threaten  the  Japanese  coast  and  erode  productive  ground. 
Granite  rock  mound  foundations  are  currently  being  laid  for  a 
tsunami  seawall  to  be  installed  in  Kamaishi  Bay  in  the 
northern  part  of  Honshu.  This  seawall  is  designed  to 
dissipate  the  energy  of  a  tsunami  prior  to  its  arrival  at 
shore.  The  Port  and  Harbour  Research  Institute  (PHRI)  of  the 
Ministry  of  Transportation  in  Yokosuka,  Japan,  wishes  to 
develop  a  general  method  to  accomplish  deep  water  structural 
inspection  of  seawalls,  including  the  Kamaishi  project.  This 
method  should  also  provide  supervision  of  construction  and 
quality  control,  and  should  not  involve  the  use  of  human 
divers  (Akizono,  1989) . 

Unfortunately,  there  is  not  an  "optimal"  device  to 
accomplish  the  task  that  PHRI  requires.  PHRI  is  currently 
using  human  divers  to  measure  wall  and  foundation  variations. 
This  is  a  difficult  process  due  to  the  pressurization 
requirements  of  the  human  body  and  the  short  time  that  divers 
can  be  at  the  bottom  (about  one  hour  per  day  at  a  depth  of 
sixty  meters) .  Additionally,  the  deep  sea  diver  occupation  is 
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physically  taxing  and  it  is  difficult  to  recruit  new 
personnel.  At  this  tine,  most  of  Japan's  deep  sea  divers  are 
in  their  late  30 's  or  older  (Takahashi,  1993).  Human  divers 
are  very  capable  when  at  the  depth  of  the  wall,  but  are  slow 
and  expensive. 

Using  a  robot  is  one  obvious  alternative.  There  are  two 
basic  options  in  the  design  of  such  a  robot.  First,  a 
floating  Remotely  Operated  Vehicle  (ROV)  could  be  used. 
However,  floating  vehicles  have  difficulty  maintaining  a 
stationary  position  while  keeping  a  specified  heading  in 
water.  A  floating  vehicle  has  a  poor  ability  to  accurately 
measure  objects  since  the  vehicle  is  not  stable.  This  would 
make  a  floating  robot  a  poor  choice  for  the  PHRI  measurement 
needs.  However,  a  floating  vehicle  is  an  excellent  choice  for 
camera  inspection  because  it  can  move  a  TV  camera  to  all 
viewing  aspects.  Unfortunately,  if  the  sea  floor  is  muddy,  a 
floating  robot  may  make  the  water  murky  due  to  turbulence 
induced  by  its  thrusters  used  for  maneuvering  (Robison,  1992) . 

Another  robot  type  available  is  the  walking  robot.  It 
provides  stability  in  a  stationary  position.  It  can  provide 
the  measurements  PHRI  desires.  However,  there  will  be 
limitations  on  the  camera  angles  dependent  upon  the  degrees  of 
freedom  of  the  camera  arm  and  the  arm  placement.  A  walking 
robot  does  not  muddy  the  water  because  it  does  not  stir  up  a 
soft  sea  floor.  Of  the  two  general  types  of  walking  robots, 
MmammalM  and  "insect",  the  insect  type  provides  better 
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movement  on  uneven  terrain  (Waldron,  1989).  Aquarobot  is  an 
insect  type  walking  robot. 

B.  AQUAROBOT  HISTORY 

PHRI  has  designed  three  robots  in  an  attempt  to  produce 
the  first  practical  underwater  walking  robot.  These  robots 
have  been  labeled  "Aquarobot"  by  their  creator,  PHRI  (Akizono, 
1989).  They  are  all  six-legged  articulated  robots. 

The  first,  an  experimental  model,  was  designed  in  1985. 
It  was  not  watertight  and  was  designed  to  run  ground  tests  for 
basic  research  and  as  a  software  debugger. 

The  second  Aquarobot,  the  prototype  model,  was  designed 
for  underwater  sea  floor  applications.  The  third  Aquarobot 
was  designed  as  a  lightweight  design  of  the  prototype  model. 
It  is  the  second  prototype  model  which  has  been  modeled  in 
this  thesis  (Akizono,  1989) . 

C.  DESCRIPTION 

The  prototype  Aquarobot  is  a  walking  ROV  designed  to 
follow  a  path  determined  from  navigation  beacons  using  a  gait 
algorithm  computed  by  a  control  station  on  a  barge  on  the 
surface,  and  passed  to  the  robot  via  a  tether.  It  is  a  six¬ 
legged  articulated  "insect  type"  robot  equipped  with  one  arm 
used  to  move  and  aim  a  video  camera  (Akizono,  1989). 

The  aquarobot  consists  of  a  hexagonal  body  and  six  legs. 
The  body  is  constructed  of  anti -corrosive  aluminum.  Each  leg 
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has  three  rotary  joints  that  provide  three  degrees  of  freedom. 
Additionally,  each  leg  has  a  disc-shaped  foot  pad  that  is 
attached  to  the  leg  with  a  freely  rotating  ball  joint.  The 
foot  pads  are  not  position  controlled,  but  are  oriented  by  a 
combination  of  gravity,  the  terrain  surface,  and  hydrodynamic 
effects  acting  on  the  ball  joint  connection.  Figure  2.1 
depicts  the  Aquarobot  and  its  leg  structure. 
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Figure  3 . 1 

Photograph  of  Aquarobot 


Each  leg  joint  of  Aquarobot  is  controlled  by  the  computer 
via  a  DC  motor  that  drives  a  reduction  gear.  The  reduction 
gear  consists  of  a  harmonic  gear  and  a  pair  of  beveled  gears. 
This  drive  method  is  known  as  a  semi-direct  drive  mechanism 
(Akizono,  1989) .  These  motors  and  gears  are  located  within 
the  legs. 
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Each  of  the  eighteen  motors  are  driven  by  DC  power.  There 
is  one  motor  driver  per  motor,  each  located  on  the  barge 
controlling  Aquarobot.  The  motor  driver  sends  the  motor  a 
voltage  computed  from  pulse  information  it  receives. 
Specifically,  the  motor  driver  contains  a  pulse  counter  which 
counts  up  for  pulses  received  from  the  computer  and  counts 
down  when  pulses  are  received  from  encoded  motor  output 
feedback.  The  motor  driver  provides  the  necessary  voltage  to 
the  motor  to  drive  the  counter  toward  zero.  Thus,  the 
motor/driver  system  uses  a  simple  position  feedback  method. 
(Akizono,  1989) 

There  are  two  inclinometers  and  one  gyrocompass  (Anon, 
1992)  on  the  body  of  Aquarobot.  Each  foot  has  a  pressure 
sensitive  touch  sensor.  These  sensors  provide  foot  contact 
and  body  orientation  information  to  the  computer  (Akizono, 
1989) .  To  measure  the  absolute  elevation  of  selected  points 
on  a  rock  mound  foundation,  one  leg  of  Aquarobot  is  also 
furnished  with  an  accurate  depth  cell  located  just  above  the 
foot  (Takahashi,  1993) . 

The  computer  system,  located  on  the  barge,  provides 
walking  algorithms  and  operating  programs.  It  is  a  16-bit 
controller.  The  interface  is  provided  by  two  integrated 
circuit  boards:  an  input/output  board  and  an  A/D  converter 
board.  The  input/output  board  sends  pulses  to  the  motor 
driver  and  receives  touch  sensor  status  and  joint  rotation 
pulses  from  the  legs.  The  A/D  converter  receives  the 
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gyrocompass,  depth  cell,  and  inclination  sensor  feedback. 
Individual  leg  motions  thus  are  performed  using  hardware 
controls,  while  top  level  motion  control  and  path  planning  is 
controlled  by  software.  (Akizono,  1989) 

The  information  bus  has  changed  throughout  Aquarobot's 
evolution.  The  tether  for  the  experimental  model  consisted  of 
copper  wire.  The  two  later  models  have  optical  fiber  links 
with  optical/electric  converters  in  the  body  and  control  unit. 
However,  all  models  contain  eighteen  copper  wires  to  carry 
current  to  individual  motors,  resulting  in  a  rather  large 
cable  cross  section  (four  centimeters) .  (Iwasaki,  1987) 

The  computer  software  is  currently  written  in  BASIC.  The 
operating  program  receives  the  walking  commands  from  the  gait 
algorithm  and  simultaneously  translates  them  to  the  motor 
drivers  in  pulse  form. 

The  prototype  model's  video  camera  arm  has  three  rotary 
joints.  Cameras  may  also  have  independent  pan  and  tilt 
control.  The  arm  is  also  equipped  with  an  ultrasonic  ranging 
device.  Using  this  device,  scales  can  be  projected  on  the 
camera  screen  so  that  measurements  of  an  object  can  be 
interpreted  in  conjunction  with  its  range  from  Aquarobot  to 
determine  actual  dimensions. 

The  prototype  model  also  has  a  relative  navigation 
capability  which  uses  a  transponder  system.  This  system 
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measures  its  position  in  cartesian  coordinates,  based  upon 
triangulation  of  signals  received  from  beacons  placed  in  the 
vicinity  of  Aquarobot  at  known  locations.  (Akizono,  1989) 

D.  CURRENT  USE  IN  JAPAN 

The  prototype  Aquarobot  has  successfully  walked 
underwater.  It's  current  maximum  walking  speed  on  uneven  sea 
bed  is  approximately  one  meter  per  minute.  While  this  speed 
is  judged  to  be  acceptable,  Aquarobot  has  not  been  put  to 
practical  use  because  human  divers  are  still  able  to  perform 
its  function  at  a  lower  cost.  (Takahashi,  1993) 

E.  POSSIBLE  AQUAROBOT  IMPROVEMENTS 

Aquarobot  could  be  improved  in  many  ways.  The  physical 
characteristics  of  the  tether  and  the  resultant  effects  of 
currents  on  it  is  an  area  where  substantial  improvements  are 
possible.  The  tether  could  be  decreased  from  its  currently 
large  circumference  and  bulky  appearance.  This  could  be  done 
by  improving  the  motor  controllers  and  placing  them  in  the 
vehicle.  In  this  way,  the  eighteen  wires  in  the  cable 
carrying  motor  currents  could  be  replaced  by  a  single  two 
conductor  power  cable.  Additionally,  the  computer  software 
could  be  optimized  to  provide  faster  and  more  flexible  code. 
New  technology  in  integrated  circuits  should  be  incorporated 
to  generally  decrease  component  size  and  power  requirements. 
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7.  SUMMARY 


Aquarobot  represents  a  major  advancement  in  the  field  of 
walking  robots.  Aquarobot 's  design  was  influenced  by  the 
mission  it  was  to  accomplish.  This  is  not  often  the  case  in 
robot  design.  Usually,  a  robot  is  designed  from  a  research 
standpoint  and  then  may  be  used  in  a  "real  life"  application. 
When  an  application  is  driving  the  technology,  robotics 
advancement  looks  at  the  problem  from  a  new  perspective  and 
new  and  varied  designs  can  be  anticipated.  The  algorithms 
required  to  calculate  the  leg  and  body  positions  of  Aquarobot 
are  described  in  the  next  chapter  of  this  thesis. 
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IV.  KINEMATICS  MODEL 

A.  INTRODUCTION 

Robots  typically  consist  of  one  or  more  "limbs"  which  are 
technically  defined  as  mechanical  manipulators.  These 
manipulators  provide  the  robot  with  the  capability  to  grasp, 
walk,  or  perform  some  other  task.  To  control  the  robot 
appendages  with  commands  to  move  to  a  desired  location, 
knowledge  from  the  field  of  physics  and  engineering  that 
describes  motion  of  rigid  bodies  is  needed.  This  field  is 
known  as  kinematics.  Kinematics  is  "...  the  science  of  motion 
which  treats  motion  without  regard  to  the  forces  which  cause 
it"  (Craig,  1989,  p.6).  Kinematics  allow  all  geometric 

properties  of  the  motion  to  be  defined. 

Forward  kinematics  computes  the  Cartesian  space  position 
and  orientation  of  the  manipulator  links  from  a  set  of 
parameters  which  describe  the  manipulator  using  angles  and 
lengths.  The  orientation  is  often  described  as  azimuth,  roll, 
and  elevation.  Inverse  kinematics  solves  for  the  manipulator 
parameters  when  the  Cartesian  space  and  orientation  are  known. 

B.  LINKAGE  AND  COMPONENT  DESCRIPTION 

Manipulators  consist  of  nearly  rigid  links  which  are 
connected  at  joints.  There  are  two  simple  types  of  joints: 
sliding  (prismatic)  and  rotary.  The  joints  are  designated  by 
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number  beginning  from  the  base,  usually  labeled  joint  0 
(Craig,  1989) .  The  base  is  also  sometimes  considered  to  be  the 
most  inboard  link.  The  free  end  of  the  links  is  the  most 
outboard  link  and  is  often  called  the  end-effector.  It  is  at 
the  end-effector  that  the  robot's  work  is  performed.  Often  the 
end-effector  is  a  grasping  device  or  a  foot  pad. 

Kinematics  considers  each  link  to  be  a  purely  rigid  body 
(Craig,  1989).  In  reality,  description  of  a  manipulator's 
links  requires  many  variables  to  be  considered  during  the 
design  process.  Some  variables  include  the  material  used  for 
construction,  the  link  strength,  stiffness,  length,  and  the 
manipulator  weight. 

Kinematic  algorithms  are  designed  to  define  the  position 
and  orientation  of  all  manipulators  regardless  of  their 
geometric  complexity.  This  is  accomplished  by  carefully 
defining  joint  coordinate  axes  called  frames  and  arranging 
their  alignments  using  standard  parameters  that  describe  the 
adjacent  link  relationships  (Craig,  1989) . 

C.  KINEMATICS  PARAMETER  DEFINITIONS 

A  frame  is  attached  to  each  joint  with  the  Z-axis 
coincident  with  the  joint  motion  axis.  The  X-axis  of  the 
frame  is  directed  from  a  link's  inboard  joint  towards  its 
outboard  joint  to  intersect  that  joint's  axis,  and  is  mutually 
perpendicular  to  both  Z-axes. 
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Four  parameters  are  needed  in  the  kinematic  algorithms. 
The  first,  link  length,  is  the  distance  along  the  X-axis 
between  the  joints  of  a  given  link.  The  second  is  link  twist. 
This  is  the  angle  necessary  to  rotate  the  inboard  Z-axis  to  be 
parallel  to  the  outboard  Z-axis. 

The  third  parameter  is  link  offset.  It  is  the  distance 
measured  at  the  inboard  link  axis  from  the  preceding  link  X- 
axis  to  the  current  X-axis.  The  final  parameter  is  the 
rotation  at  this  joint  from  the  previous  link  X-axis  to  the 
current  link  X-axis.  This  is  known  as  the  joint  angle. 

D.  CRAIG  VERSUS  DANEVIT-HARTENBERG  METHOD  COMPARISON 

Forward  kinematics  determines  the  cumulative  effect  of 
joint  motions  on  the  entire  link  chain.  This  cumulative 
effect  can  be  accomplished  by  a  number  of  methods.  Two  common 
methods,  Craig  and  Danevit-Hartenberg ,  are  related  in  their 
approach  but  differ  in  their  setup  (Spong,  1989) . 

To  begin  with,  the  manipulator  must  be  inspected.  The 
frames  must  be  placed  with  the  proper  orientation.  The  four 
parameters  discussed  above  must  then  be  determined.  These 
parameters  are  identical  for  both  methods;  however,  the 
numbering  of  the  joint  frames  varies. 

The  Craig  method  numbers  the  links  beginning  with  zero  at 
the  most  inboard  link.  The  base  joint  is  numbered  joint  0. 
This  produces  a  numbering  system  where  the  link  and  the  link's 
inboard  joint  have  the  same  index  number  (Craig,  1989) .  An 
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example  of  this  notation  is  pictured  in  Figure  4.1.  The  base 
(joint  0)  inboard  link  length  and  inboard  link  twist  are  both 
defined  as  zero. 


joint  i  Joint  i+1 


Figure  4.1 

Craig  Method  Frame  and  Parameter  Assignment 
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The  Danevit-Hartenberg  (DH)  notation  differs  from  the 
other  method.  In  this  method,  the  first  link,  attached  to  the 
base  joint,  is  labeled  link  1.  The  base  joint  is  labeled 
joint  0.  This  produces  a  numbering  system  along  the  link 
chain  in  which  the  link  and  the  link's  outboard  joint  have  the 
same  index  number  (Spong,  1989) .  An  example  of  this  method  is 
pictured  in  Figure  4.2. 


Figure  4.2 

Danevit-Hartenberg  Frame  and  Parameter  Assignment 
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These  methods  use  related  conventions  for  manipulating 
these  parameters;  however,  the  algorithms  are  different.  The  » 

cumulative  effect  of  the  links  are  defined  within  a  matrix 
known  as  the  transformation  matrix  (Craig,  1989) .  The 
transformation  matrix  differs  for  the  two  methods  addressed.  » 

The  Craig  method  uses  a  transformation  matrix  (known  as 
the  T  matrix)  to  define  the  outboard  joint  location  on  a  link 
relative  to  the  inboard  joint.  The  T  matrix  is  defined  as  » 

(Craig,  1989,  p.84): 
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The  subscript  of  the  T  matrix  label  describes  which  joint  is 
being  defined.  The  superscript  of  the  T  matrix  describes  the 
link  to  which  the  matrix  is  referenced. 

The  Danevit-Hartenberg  method  uses  a  transformation  matrix 
(known  as  the  A  matrix)  to  define  the  location  of  the  inboard 
joint  on  a  link  relative  to  the  outboard  joint.  That  is,  the 
coordinate  origin  for  a  link  is  located  at  its  outboard  joint 
for  the  DH  method,  while  it  is  at  the  inboard  joint  in  the 
Craig  method.  The  A  matrix  is  defined  as  (Spong,  1989, 

p. 66) : 
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The  subscript  and  superscript  of  the  A  matrix  are  defined  the 
same  as  the  T  matrix  above.  However,  by  convention,  the  index 
is  transposed. 

These  transformation  matrices  provide  information  on  the 
rotation  and  translation  needed  to  superimpose  the  frame  being 
transformed  to  the  relative  frame.  The  rotation  information 
is  the  top  left  3x3  sub  matrix  in  the  transformation  matrix. 
The  translation  information  is  in  the  right  column  in  the 
first  three  rows. 

The  base  joint  is  aligned  with  the  coordinates  that  the 
designer  would  like  to  use  to  reference  the  link  positions. 
Usually,  for  fixed  base  manipulators,  the  base  joint  axis  is 
aligned  with  the  Earth's  coordinates.  To  transform  the  joint 
in  question,  the  transformation  matrices  need  to  be  multiplied 
together  (Craig,  1989) .  For  example: 

4°T  =  ,®T  *  2*T  *  32T  *  4*T  (4.5) 

04A  =  o'A  *  ,2A  *  23A  *  34A  (4.6) 

E.  AQUAROBOT  KINEMATICS 

Aquarobot's  six  legs  are  identical  manipulators  except  for 
their  angle  off  of  the  body's  forward  axis.  In  order  to 
simplify  the  leg  parameters  of  the  first  link,  an  imaginary 
link  was  constructed  from  the  body's  center  to  the  point  where 
the  leg  joins  the  body.  This  makes  the  body's  center  the  base 
joint.  The  Craig  method  will  be  used  in  this  thesis  to  solve 
the  kinematic  equations  for  Aquarobot's  legs. 


1.  Aquarobot  Lag  Parameters 


Common  symbols  exist  for  the  parameters.  They  are: 
link  length  (aj),  link  twist  (or,),  link  offset  (df)  ,  and  joint 
angle  (8,)  .  Figure  4.3  shows  one  Aquarobot  leg  with  the 
imaginary  leg  link  included. 


JT  2 


JT  3 


Figure  4.3 

Aquarobot  Frame  Descriptions  Of  One  Leg  and  The  Body 


L BO  e 


Figure  4.4 

Top  View  of  Aquarobot  Showing  First  Two  Angles 
and  Axes  for  Leg  Six 

The  parameters  for  Aquarobot 's  legs  are  shown  in  Table 
4.1  telow.  Note  that  the  joint  angle  of  the  base  (i  =  o)  is 
the  only  fixed  parameter  that  varies  among  the  legs.  The  joint 
angle  range  for  the  other  joints  common  to  all  legs  are  given. 
These  limits  are  the  physical  joint  ranges.  Joint  four  does 
not  have  a  frame  designated  because  it  is  a  passive  ball 
joint. 
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TABLE  4.1 


AQUAROBOT  KINEMATICS  PARAMETERS 
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2 .  Transformation  Matrices 

The  transformation  matrices  of  the  Aquarobot  legs  were 
constructed  using  Table  4.1  above.  The  Craig  method 
transformation  matrix  template  was  used.  The  computed  link 
transformations  are  thus: 


T 


°  T 


(4.7) 


(4.8) 
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a  T 


3  T 


O  20 

i  o 

o  o 

O  1 


O  50 

O  O 

1  O 

O  X 


o  xoo 
o  o 

X  o 

O  X 


(4.9) 


(4.10) 


(4.11) 


Multiplying  the  T  matrices  together  provides  the  joint 
coordinates  in  reference  to  the  body's  center.  These 
Cartesian  coordinates  are  found  in  the  third  column  of  the 
product  of  the  T  matrix  multiplication. 

The  T  matrix  product  of  each  joint  is  called  the 
Homogeneous  Transformation  matrix  (i.e.,  H  matrix).  To 
determine  the  next  outboard  joint's  orientation  based  upon  the 
reference  frame,  the  H  matrix  of  the  current  (relatively 
inboard)  joint  is  multiplied  by  the  outboard  joint's  T  matrix. 
The  joint's  H  matrix  provides  the  orientation  from  the 
reference  frame  outboard  to  that  joint. 
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The  H  matrix  for  the  body  provides  orientation  of  the 
body  frame  (and,  in  turn,  its  outboard  joints)  to  the  fixed 
reference  frame  which  is  usually  a  designated  point  on  Earth. 
The  initial  orientation  information  required  is  azimuth, 
elevation,  roll,  and  translation  from  the  reference's  origin. 
Elevation  is  defined  as  rotation  of  the  body  X-axis  above  or 
below  the  horizontal  plane.  Azimuth  is  the  rotation  of  this 
axis  away  from  north  about  a  downward  directed  axis.  Roll  is 
rotation  about  the  body  X-axis  after  azimuth  and  elevation 
rotations  have  been  accomplished.  The  H  matrix  is  defined  as 
(Craig,  .1989,  p.  46): 


c(a)c(e)  c{a)8(e)s(r)-fl{a)c(r) 

a(a)c(e)  8(a)s(e)a{r)+c{a)c(r) 
-s(e)  c (e) s (r) 

0  0 


where  a  =  azimuth  e  =  elevation  r  =  roll 

When  the  body  moves,  its  H  matrix  relates  its  body 
coordinate  system  to  the  world  coordinate  system.  The 
cumulative  effect  of  the  body's  motion  is  transferred  to  the 
individual  links  via  the  H-matrix  and  continues  to  be 
transferred  outboard  in  this  manner. 


c(a)s(e)c(r)+s(a)8(r) 

a(a)8(e)c(r)-c(a)8(r) 

c(e)c(r) 

0 


y 

X 

1 


(4. 
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F.  INVERSE  KINEMATICS 

Inverse  Kinematics  provides  the  parameter  values  needed  to 
move  the  joints  to  a  desired  position.  The  transformation 
matrix  products  above  are  equated  to  the  generic 
transformation  matrix  to  make  a  set  of  nonlinear  equations 
(Craig,  1989,  p.  123). 


T 


R  T 


X 

y 

*» 

rsa 

z 

(4.13) 

0 

0 

0 

1 

These  equations  are  solved  simultaneously  for  the  unknown 
parameters  (joint  rotation  in  the  case  of  Aquarobot) .  The 
inverse  kinematics  of  Aquarobot  are  solved  in  another  thesis 
(Schue,  1993)  .  There  are  occasions  when  two  solutions  for  a 
parameter  are  possible  (Craig,  1989) . 
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O.  SUMMARY 


Aquarobot's  design  uses  rotating  joints.  Rotating  joints 
have  an  advantage  over  sliding  joints  in  that  they  generally 
provide  increased  dexterity.  Additionally,  such  joints  can 
usually  be  made  smaller  than  sliding  links  (Spong,  1989) . 
They  are  also  easier  to  waterproof  for  an  underwater  walking 
robot . 

Kinematics  analysis  permits  Aquarobot's  foot  positions  to 
be  easily  determined  using  successive  transformations. 
Kinematics  equations  can  be  manipulated  quickly  using 
computers.  Object  oriented  programming  simplifies  the 
numerous  transformations  necessary  for  an  intricate  multi-link 
system.  Object  oriented  programming  is  discussed  in  the  next 
chapter . 
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V.  OBJECT  ORIENTED  PROGRAMMING 


A.  INTRODUCTION 

Object  Oriented  Programming  emphasizes  the  subjects  which 
operations  act  upon  in  contrast  to  the  traditional  programming 
method  of  emphasizing  the  algorithms  and  the  order  necessary 
to  execute  them  (Booch  1991) .  The  Object  Oriented  (OO) 
designer  constructs  his  modules  of  code  based  on  items  (known 
as  objects) .  These  objects  need  not  in  every  case  accomplish 
anything  significant ,  but  they  do  at  a  minimum  provide 
encapsulated  data.  Other  designers  construct  their  modules 
based  upon  the  data  and  algorithms  that  are  associated  with 
such  blocks . 

OO  code  permits  the  designer  to  produce  elementary 
components  and  then  link  these  objects  together  to  produce  a 
complex  system.  This  parallels  the  thought  process  that 
humans  use  to  think  of  objects  around  them. 

OO  code  provides  two  structures,  object  and  class. 
Classes  are  the  blueprints  of  a  component  and  exist  in  a  "kind 
of"  hierarchy.  Objects  are  the  actual  produced  copy  of  the 
object  (instances  of  classes)  and  may  exist  in  a  "part  of" 
hierarchy  in  relation  to  other  objects. 
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B.  CLA88  DEFINITION  AMD  CLASS  HIERARCHIES 

Classes  are  the  building  blocks  or  key  designs  of  a 
system.  They  are  synonymous  with  a  factory's  product 
blueprints.  Classes  provide  the  ability  to  make  many  modules 
(objects)  that  are  designed  identically.  Each  object,  when 
made,  provides  the  "essence"  of  the  class  (Fink,  1992) . 
Classes  are  static,  and  the  information,  known  as  fields,  of 
the  class  are  fixed.  The  class  definition  provides  a  template 
for  the  production  of  objects. 

A  class  can  inherit  from  one  or  more  superclasses.  The 
inheriting  class  is  known  as  a  subclass.  A  class  can  also  have 
subclasses  which  consists  of  it  and  additional  information. 
Each  senior,  top  level  module,  represents  one  of  the  most 
general  designs  in  a  system. 

Class  structures  may  include  object  fields,  also  known  as 
slots .  from  multiple  superclasses  with  subclasses  created 
using  some  priority  scheme  or  other  means  to  resolve  conflicts 
(Booch,  1991) .  These  class  frameworks  are  transferred  to  the 
objects  that  are  produced. 

Multiple  class  structures  form  a  design.  Seldom  does  one 
class  concisely  define  a  system.  The  class  hierarchy  permits 
all  nuances  desired,  regardless  of  significance,  to  be  defined 
as  the  designer  chooses  at  that  level. 

The  classes  designed  to  serve  as  templates  are  defined  as 
concrete  classes.  They  are  expected  to  have  objects 
instantiated  from  them  (de  Paula  and  Nelson,  1991) . 
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Not  all  classes  are  designed  as  templates  for  actual 
object  instantiation.  These  are  called  abstract  classes.  I 

They  are  higher  level  classes  which  hold  knowledge  that  all  of 
their  subclasses  have  in  common.  Abstract  classes  reduce 
duplication  of  common  knowledge  (Wu,  1991) .  They  are  written  l 

so  that  multiple  subclasses  can  inherit  from  them. 

A  class  capability  provides  two  of  the  three  features 
necessary  for  OO  programming.  First,  it  provides  for  a  design  I 

to  be  defined  in  a  generic  format.  Second,  it  allows  the 
designs  to  be  modularized  at  the  most  general  level,  yet  still 
provides  for  a  relationship  framework  where  additional  design  I 

features  can  be  added  in  subclasses  (Booch,  1991). 

C.  OBJECT  DEFINITION  AND  OBJECT  HIERARCHIES 

I 

Objects  are  the  useable  products  of  OOP  and  provide  the 
third  capability  needed  for  utilization  of  this  technology. 

They  are  concrete  software  entities  that  can  be  manipulated. 

I 

An  object  has  all  of  the  properties  of  its  class.  All  objects 
produced  from  the  same  class  contain  identical  fields  and 
functions,  yet  it  is  important  to  understand  that  each  object 

I 

has  its  own  identity  and  its  own  name  when  produced.  It  is  by 
this  name  that  the  object  is  addressed  within  the  code.  The 
objects  are  facsimiles  of  the  class  and  its  behavior  and 

I 

fields.  However,  they  may  be  elaborated  individually  (Eckel, 

1989) . 
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An  object  is  constructed  by  creating  an  instance  of  the 
class  desired.  All  superclasses  and  their  defined  functions, 
also  known  as  methods ,  are  available  to  the  object.  It  is  not 
necessary  to  have  an  object  for  every  class.  A  class  may  have 
zero,  one,  or  multiple  instances  of  itself  (de  Paula  and 
Nelson,  1991) . 

An  instance  of  an  object  may  be  produced  by  two  different 
schemes.  First,  an  object  may  be  instantiated  within  the  main 
program.  This  object  may  subsequently  be  addressed  by  its 
created  name.  Such  an  object  is  a  top  level  object  within  the 
software.  It  may  also  be  considered  a  subobiect  if  it  is 
used  as  part  of  a  larger  composite  object.  The  second  type  of 
object,  a  dependent  object,  is  instantiated  directly  and 
automatically  as  a  part  of  another  object.  The  dependent 
object  is  considered  a  component  of  the  object  it  is 
instantiated  within  and  has  no  independent  name.  A  dependent 
object  is  instantiated  during  construction  of  the  main  object 
and  is  destroyed  when  the  main  object  is  destroyed.  For 
example,  a  sports  car,  when  produced,  can  be  thought  of  as  an 
object  and  its  components,  such  as  the  doors,  can  be 
considered  dependent  objects  since  they  are  made  during  the 
sports  car's  construction  and  are  legally  part  of  the  vehicle. 

Objects,  when  instantiated,  acquire  all  of  the  fields  of 
the  class,  but  the  values  of  these  characteristics  may  be 
initialized  individually  during  the  construction  of  the 
individual  objects.  For  example,  this  permits  numerous 
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objects  of  the  same  class  to  be  instantiated  yet  have 
different  measurements  or  characteristics. 

An  object  may  change  its  slot  values  during  its  existence. 
This  provides  an  object  with  a  history.  An  object  may  be 
created  and  destroyed.  The  object's  functions  can  not  be 
violated.  Objects  perform  functions  by  sending  requests. 
These  functions  are  designed  within  the  class  structure  and 
are  applicable  to  the  objects  instantiated  from  the  specific 
class  or  its  class  hierarchy.  This  capability  to  perform 
functions  enables  an  object  to  be  much  more  than  a  data 
structure . 

Functions,  history,  and  "lifetime"  characteristics  provide 
objects  with  state,  behavior,  and  identity  (Booch,  1991) . 
This  parallels  their  real-world  counterparts. 

D.  INHERITANCE 

Inheritance  is  defined  as  a  "...  mechanism  for  resource 
sharing  in  hierarchies"  (Wegner,  1987,  p.  169).  It  is  a 
unique  contribution  of  00  languages.  Inheritance  provides  an 
easy  way  to  create  objects  that  are  very  similar,  although 
individual  instances  may  have  some  differences  (Stefik,  1986)  . 
The  subclass  is  a  specialization  that  augments  or  alters  the 
structure  and  behavior  of  the  inherited  class.  It  inherits  all 
functions  and  methods  defined  for  its  superclasses.  This 
includes  all  attributes  that  the  superclass  inherited  form  its 
superclass  (Wegner,  1987) .  A  subclass  may  have  fields  or 
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methods  which  modify,  elaborate,  or  add  to  those  inherited  by 
its  superclass  (Booch  1991) . 

When  a  subclass  has  one  superclass,  this  is  called  single 
inheritance.  Two  or  more  superclasses  defines  multiple 
inheritance.  Inheritance  is  a  class  relationship  rather  than 
an  object  relationship. 

E.  CLASS  -\ND  OBJECT  DIAGRAMS 

Class  and  object  diagrams  provide  a  logical  view  of  a 
system.  The  difference  between  object  and  class  diagrams  is 
an  important  concept  in  00  design. 

Class  diagrams  are  built  on  interclass  relationships 
involving  inheritance.  Superclasses  and  subclasses  describe 
these  diagrams.  Class  utilities  provide  a  special 
relationship  within  the  class  diagram.  A  class  utility  is  an 
abstract  class  type  which  provides  functions  that  do  not 
belong  to  one  particular  class  but  are  accessible  to  all. 
Figure  5.1  displays  an  example  of  a  representative  class 
diagram.  Note  that  there  is  only  one  of  each  class  in  the 
class  diagram. 

Figure  5.1  displays  an  automobile  class  diagram.  The  top 
level  module,  the  automobile  class,  is  the  superclass.  In 
this  class  structure,  subclasses  are  necessary.  Two  subclass 
levels  are  necessary  in  order  to  reach  a  concrete  class.  The 
Porsche  can  be  produced  but  a  sports  car  is  an  abstract  class 
that  can  not  be  instantiated. 
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SUPERCLASS 


a-^nd-of 


SUBCLASS 


Figure  5.1 

Example  Class  Diagram 
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Object  diagrams  "...  show  the  existence  of  objects  and 
their  relationship  in  the  logical  design  of  the  system  ..." 
(Booch,  1991,  p.  169).  An  object  diagram  shows  the 
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object  is  defined  as  an  object  linked  to 


» 


other  objects  by  part-of  relationships.  Parts  of 
composite  objects  may  be  subobjects  or  dependent  objects. 


the 


F.  CONCLUSIONS  ABOUT  OBJECT  ORIENTED  DESIGN 

OO  Programming  allows  designers  to  begin  with  a  simple  or 
general  system.  This  principle  of  beginning  with  a  large, 
less  specific  class  or  object  is  similar  to  human  perception. 
First,  humans  determine  what  the  overall  item  in  questions  is. 
For  example,  a  person  may  look  at  a  car.  He  or  she  is  likely 
to  note  the  model  of  the  car  at  that  time.  Then,  smaller 
"part  of"  subsystems  of  the  car  or  specific  slot  values  may  be 
inspected.  For  example,  the  year  the  car  was  produced  or  the 
air  conditioning  system  may  be  looked  into.  OO  code  permits 
the  programmer  to  define  subclasses  or  components  when  they 
are  needed  or  as  the  system  is  elaborated  upon.  This  allows 
the  programming  to  be  accomplished  in  small  increments.  OO 
designs  allow  attention  to  be  focused  on  the  appearance  and 
external  capabilities  of  objects  instead  of  on  software 
implementation  details.  This  prevents  too  much  information 
from  "cluttering"  the  user's  view  (Snyder,  1986).  Another 
advantage  to  OO  Programming  is  the  capability  to  improve  or 
alter  class  slots  as  the  system  changes  or  as  corrections  are 
needed.  This  resilience  and  the  capability  to  reuse  small 
subsystems  in  multiple  objects  makes  00  code  economical  (Booch 
1991)  . 
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00  Programming  encourages  reuse  of  entire  software  class 
hierarchies.  The  modular  design  of  OO  code  permits  new  users 
to  incorporate  existing  code  without  having  to  retest 
functions  or  redesign  code.  This  extendibility  of  code  life 
reduces  a  designers  programming  time.  The  modular  design  of 
class  code  permits  the  user  to  use  the  functions  of  the  class 
without  requiring  an  intimate  detailed  knowledge  of  the 
function's  inner  workings. 

A  disadvantage  of  00  Programming  is  that  classes  may  be 
designed  without  placing  functions  in  the  most  general 
superclass.  This  causes  identical  functions  to  be  defined  in 
numerous  subclasses  and  increases  complexity.  Repetition 
should  be  minimized  by  placing  common  functions  of  two  or  more 
classes  in  a  superclass.  Of  course  this  can  be  an  iterative 
process  with  common  properties  or  methods  being  factored  out 
and  moved  upward  in  a  class  hierarchy  as  they  are  noted. 

Q.  SUMMARY 

Human  capacity  is  limited  in  its  capability  to  grasp 
complex  systems.  OO  code  enables  a  person  to  look  at  a  complex 
system  as  a  collection  of  various  subsystems.  It  also 
provides  the  capability  to  only  look  at  areas  of  interest 
within  an  object/class. 
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Object  Oriented  programming  provides  software  that  is 
"malleable".  This  is  directly  due  to  class  structure  and 
inheritance,  a  unique  characteristic  of  this  code  (Booch, 
1991)  . 

The  difference  between  the  class  and  object  structures  is 
a  subtle  but  important  one.  An  object  is  an  instance  of  a 
class  and  the  object  may  be  created  and  destroyed  within  a 
program.  A  class  is  designed  but  it  is  static  when  a  program 
is  executed  (Booch,  1991) . 

The  long  life  span,  maintainability,  and  flexibility  in 
application  of  Object  Oriented  code  makes  it  the  premiere 
choice  when  a  design  with  multiple  subsystems  is  desired.  The 
greatest  hindrance  of  Object-Oriented  Programming's  potential 
to  be  the  popular  choice  in  industrial  design  is  its  current 
lack  of  widely  excepted  standards.  However,  there  seems  to  be 
considerable  consensus  on  OOP's  primary  concepts.  The  next 
chapter  describes  oo  languages  suitable  for  development  of  an 
Aquarobot  kinematic  simulation. 
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VI.  OBJECT  ORIENTED  PROGRAMMING  LANGUAGES 
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A.  INTRODUCTION 

Not  all  computer  language.;  are  able  to  support  OOP.  Four 
predominant  languages  with  00  capability  are  CLOS,  C++,  Object 
Pascal,  and  Smalltalk.  Aguarobot  is  designed  using  CLOS  and 
C++.  Aquarobot's  class  and  object  hierarchies  are  described 
and  then  created  with  CLOS  and  C++  code. 

B.  DESCRIPTION  OF  CLOS 
1 .  History 

LISP  evolved  in  the  late  1950 's  and  was  named  for  its 
performance  method:  List  Processing  (Winston,  1989) .  The 
fundamental  element  in  LISP  is  a  wordlike  object  known  as  an 
atom.  A  group  of  atoms  (similar  to  a  sentence  of  words)  is 
known  as  a  list  (Winston,  1989)  .  It  is  these  lists  which  LISP 
is  designed  to  manipulate.  LISP  allows  for  lists  to  be  added 
to  or  deleted  from  indefinitely.  Specific  atoms  may  be 
extracted  or  manipulated  using  LISP  created  or  library 
functions. 

Common  LISP  was  officially  designed  in  1984  to 
accumulate  the  existing  LISP  variations  into  one  standard 
version.  This  standardization  was  advantageous  for  academic 
and  industrial  use  (Steele,  1990).  Common  LISP  was  then 
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extended  to  provide  00  capability  and  this  extension  is  known 
as  CLOS  (£ommon  LISP  Object  System)  (Steele, 1990) . 


CLOS  (pronounced  see-loss)  permits  each  class  to  have 
local  and  shared  slots.  These  slots  can  be  directly  accessed 
and  modified  by  the  programmer  (Winston,  1989) .  CLOS  provides 
for  multiple  inheritance  within  the  class  hierarchy. 
Conflicts  in  multiple  slot  inheritance  is  avoided  due  to 
conflict  precedences  which  define  the  first  superclass  listed 
as  superior  (Fink,  1992) . 

2.  Benefits  in  the  Kinematics  soluti.^ 

CLOS  (and  therefore  LISP)  has  many  advantages  in  the 
robot  kinematics  solution.  CLOS  operates  in  an  interpretive 
environment  that  facilitates  interactive  programming, 
providing  information  such  as  variable  status,  with  rapid 
response  (Winston,  1989) .  This  capability  for  immediate 
answers  to  a  drafter's  questions  provides  ease  in  debugging 
as  well  as  the  drafting  of  programs  (Winston,  1989) .  Lists 
are  addressed  and  manipulated  using  programmer  defined 
symbolic  names  which  generally  tend  to  decrease  the  code 
length  and  improve  readability  (Keene,  1989) . 

The  symbol  manipulation  and  interactive  capability  of 
CLOS  simplify  the  kinematics  solution.  Each  limb's  joints  can 
be  placed  in  one  list  or  each  parameter  can  be  designed  as  an 
atom  in  a  joint  list.  CLOS  code  provides  compact  code  ior 
extensive  systems  as  well  as  functions  that  are  easy  to  read. 
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More  generally,  academic  institutions  and  industries  can 
create  complex  systems  with  big  programs  that  run  faster  and 
are  less  expensive  due  to  the  compact  code  (Winston,  1989) . 

CLOS  also  incorporates  the  natural  language 
orientation  of  LISP.  Class  and  object  structures  and  their 
slots  and  values  are  easily  understood  (Keene,  1989) . 

C.  DESCRIPTION  OF  C++ 

1.  History 

C++  was  designed  in  1986  at  AT&T  Bell  Laboratories  by 
Bjarne  Stroustrup,  and  is  a  superset  of  the  C  language  (Booch, 
1991) .  C++  incorporates  the  programming  abilities  of  C  and 

adds  OO  properties  as  well  as  type  checking  and  operator 
overload  functions.  In  1989,  C++  Version  2.0  provided 
multiple  inheritance  (Stroustrup,  1991) . 

2.  Benefits  in  the  Kinematics  Solution 

C++  is  similar  in  format  to  many  presently  popular 
languages  such  as  Ada  and  C.  The  familiar  format  is  an 
advantage  of  C++  when  an  OOP  language  is  necessary. 
Conversely,  LISP  has  a  unique  format  that  is  not  currently 
a  popular  choice  in  academic  institutions  or  industry.  This 
uniqueness  is  a  disadvantage  to  CLOS  unless  the  drafter 
understands  LISP. 

C++  permits  users  to  apply  functions  without 
necessitating  intimate  detailed  knowledge  of  the  class  inner 
workings  (Booch,  1991) .  C++  uses  a  header  file  to  provide  a 
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top  level  view  of  class  structure  and  the  functions  which 
apply  (Booch,  1991) .  Functions  must  be  defined  for  a  specific 
class  within  its  hierarchy  because  of  C++'s  strong  typing. 
Subclasses  may  alter  functions  that  their  superclass  defines. 
Common  operators  such  as  addition  (+)  and  equality  (=)  are 
generically  defined  for  common  classes  (e.g.,  integer,  double, 
array,  etc.),  but  they  must  be  redefined  in  new  classes  where 
their  use  is  desired  (e.g.,  a  matrix  class  or  link  chain) 
(Ammeraal,  1991) . 

D.  SMALLTALK  AMD  OBJECT  PASCAL  DESCRIPTIONS 

Smalltalk  and  Object  Pascal  are  also  OO  languages.  Like 
C++  and  CLOS,  Object  Pascal  provides  an  enhancement  of  the 
Pascal  language.  Object  Pascal  was  specifically  designed  to 
add  an  OO  capability  to  Pascal.  However,  Object  Pascal  is 
more  restrictive  than  C++  in  code  development  (Booch,  1991) . 
All  class  slots  are  public  so  slots  may  be  changed  while 
performing  another  class  function  (Booch,  1991) . 

Smalltalk  was  designed  as  a  pure  00  language  and  provides 
many  predefined  classes.  Unlike  Object  Pascal,  all  slots  in 
Smalltalk  are  private.  Object  Pascal  is  unique  in  that  it 
provides  an  overall  system  template  and  all  created  classes 
are  considered  subclasses  of  a  predefined  superclass  called 
"Object"  (Booch,  1991) .  Smalltalk  is  not  a  strongly  typed 
system,  therefore  a  compiler  cannot  optimize  the  code. 
Smalltalk  is  limited  to  single  inheritance  (Booch,  1991) . 
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E.  SUMMARY 


OO  capability  is  presently  a  popular  (and  seemingly 
necessary)  addition  to  current  programming  languages.  This 
development  coincides  with  the  increased  use  of  OO  in  academic 
and  industrial  system  design. 

CLOS  provides  an  easier  format  for  user's  to  read  than 
C++.  However,  C++  provides  non-list  manipulations  which  are 
often  more  efficient.  Both  languages  require  knowledge  of  the 
original  language  they  embellished  or  a  similarly  formatted 
language.  CLOS  requires  less  code  space  than  C++,  but  C++ 
usually  executes  more  efficiently,  and  may  require  less 
memory . 

CLOS  provides  dynamic  memory  allocation  and  uses  "garbage 
collection"  to  accumulate  unused  memory  space.  Activity  is 
suspended  during  garbage  collection  which  may  hinder  real  time 
calculations  in  some  garbage  collection  methods.  In  contrast, 
C++  uses  a  memory  heap  which  requires  that  memory  be  removed 
and  then  explicitly  returned  to  the  heap  when  the  memory  space 
is  no  longer  needed.  The  next  chapter  provides  a  description 
of  CLOS  and  C++  and  examples  of  their  formats  in  the  context 
of  the  Aquarobot  code  developed  in  this  thesis. 


50 


VII.  AQUAROBOT  CODE  DESCRIPTION 


A.  INTRODUCTION 

In  order  to  produce  an  Aquarobot  simulation,  each  of  the 
robot ' s  major  parts  needed  to  be  simulated.  OOP  was  chosen  as 
the  best  method  to  achieve  this  goal.  One  version  of 
Aquarobot  was  written  in  CLOS  by  Prof.  Robert  McGhee  at  the 
Naval  Postgraduate  School.  The  other  version  was  written  in 
C++  by  this  author.  In  this  chapter  of  this  thesis,  the 
object  and  class  diagrams  for  these  two  implementations  are 
presented  along  with  examples  of  the  method  each  language  uses 
to  produce  an  individual  class  and  instantiate  an  object.  The 
C++  graphics  code  is  discussed  and  examples  of  the  display  are 
included.  The  complete  CLOS  and  C++  Aquarobot  programs  are 
found  in  Appendix  A  and  B  respectively. 

B.  AQUAROBOT  CLASS  AND  OBJECT  HIERARCHIES 

The  class  hierarchies  designed  to  produce  an  Aquarobot  in 
CLOS  and  C++  are  shown  in  Figures  7.1  and  7.2  respectively. 
These  two  figures  are  not  identical,  but  there  are  major 
portions  that  are  similar. 

The  RigidBody  class  is  a  superclass  of  the  system.  Its 
subclasses  are  the  major  pieces  with  which  Aquarobot  and  its 
components  are  created.  The  AquaLeg  class  uses  the  Link 
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Figure  7 . 1 

CLOS  Aquarobot  Class  Diagram 
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Figure  7.2 

C++  Aquarobot  Class  Diagram 
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subclasses  (linkO  through  link3)  and  a  few  numerical  slots  to 
create  another  top  level  class.  The  AquaLeg  class  also 
possesses  functions  that  manipulate  an  AquaLeg  type.  The 
Matrix  class  is  unique  to  the  C++  version.  The  CLOS  version 
uses  lists  to  store  data  while  the  C++  version  uses  this 
defined  Matrix  class  and  its  functions  to  store  and  manipulate 
the  data.  The  Matrix  class  is  a  typical  example  of  a  class 
utility. 

The  object  hierarchy  used  to  instantiate  an  Aquarobot 
differs  in  the  two  versions.  Figure  7.3,  the  C++  object 
diagram,  constructs  Aquarobot  as  seven  subobjects  which  can  be 
deleted  or  reproduced  without  affecting  the  existence  of  the 
other.  Figure  7.4  displays  the  CLOS  object  diagram  that  has 
one  top  level  object  with  dependent  object  hierarchy 
containing  a  total  of  thirty-one  objects.  The  Leg  object  and 
its  dependent  subobjects  are  identical  in  each  language 
version. 

C.  AQUAROBOT  CLASS  DEFINITION  CODE 

Classes  are  defined  in  various  ways  dependent  upon  the  OO 
language  used.  There  are,  however,  many  similarities  in  class 
attributes.  For  example,  both  CLOS  and  C++  have  slots  for  the 
items  within  a  class.  The  AquaLeg  class  definition  in  both 
language  versions  is  explained  in  the  following  paragraphs. 
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CLOS  Object  Hierarchy 


1.  CLOS  Class  Description 


> 


CLOS  provides  a  template  that  contains  both  optional 
and  mandatory  information  requirements.  This  template  can 
be  found  in  CLOS  manuals.  Figure  7.5  is  the  aqua-leg  class 
defined  using  CLOS.  Eight  slots  are  defined  and  then 
initialized  using  the  ; initara  or  : initform  command.  The 
dependent  objects  shown  in  Figure  7.4  are  instantiated 
within  the  aqua-leg  class  as  linkO  through  link3  using  the 
make- instance  command.  The  linkO  class,  for  example, 
incorporates  the  superclass  slots  of  Link. 

The  functions  related  to  the  aqua-leg  class  are 
defined  outside  of  the  class  definition  in  def methods. 
"Initialize-leg"  is  a  function  which  requires  an  "aqua-leg" 
and  an  "aquarobot-body"  class  as  input.  Each  input  is  given 
a  local  variable  name  of  "leg"  and  "body"  respectively.  The 
functions  may  call  other  functions  or  change  slot  values. 
The  CLOS  code  includes  a  camera  class  because  the  code  was 
developed  on  a  Sun  workstation  while  the  C++  version  uses 
the  graphics  library  on  a  Iris  workstation. 

2.  C++  Class  DESCRIPTION 

The  C++  AquaLeg  class  is  defined  within  the  AquaLeg.H 
file  (Figure  7.6).  Like  the  CLOS  version  in  Figure  7.5,  there 
are  four  dependent  objects  that  are  slots  of  the  AquaLeg 
class.  Like  the  CLOS  version,  functions  which  are  applicable 
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(defclass  aqua-leg  () 

( (leg-attachment-angle 

:initarg  : leg-attachment -angle 
: accessor  leg-attachment -angle) 

(linkO 

:initform  (make-instance  'linkO) 

:accessor  linkO) 

(linkl 

:initform  (make-instance  'linkl) 

: accessor  linkl) 

(link2 

:initform  (make-instance  'link2) 

:accessor  link2) 

(link3 

:initform  (make-instance  ' link3) 

: accessor  link3) 

(mot ion-complete- flag 
:initform  nil 

: accessor  motion-complete-flag) 

(previous-foot -posit ion 
:initform  nil 

: accessor  previous-foot-position) 

(cur rent -foot -posit ion 
tinitform  nil 

: accessor  current-foot-position) ) ) 

(defmethod  initialize-leg  ((leg  aqua-leg)  (body  aqua robot -body) ) 
(setf  (inboard-link  (linkO  leg))  body) 

(setf  (inboard-link  (linkl  leg))  (linkO  leg)) 

(setf  (inboard-link  (link2  leg))  (linkl  leg)) 

(setf  (inboard-link  (link3  leg))  (link2  leg)) 

(rotate-link  (linkO  leg)  (leg-attachment -angle  leg)) 
(rotate-link  (linkl  leg)  (inboard- joint-angle  (linkl  leg))) 
(rotate-link  (link2  leg)  (inboard- joint -angle  (link2  leg))) 
(rotate-link  (link3  leg)  (inboard- joint-angle  (link3  leg))) 
(setf  (current-foot -position  leg) 

(near  3  (first  (transformed-node-list  (link3  leg)))))) 


Figure  7.5 

CLOS  code  Excerpt  Defining  and  Implementating 
Aquarobot  Leg  Kinematics 


class  Aqua Leg 
( 

public : 

//  these  dependent  objects  are  instantiated 
LinkO  *linkO; 

Linkl  *linkl; 

Link2  *link2; 

Link3  *link3; 

II  the  flag  is  set  to  1  if  the  motion  is  completed  without 
II  reaching  any  link  limits 
int  motion_complete_f lag; 

//  the  flag  is  set  to  1  if  the  leg  is  on  the  ground 
int  leg_support_f lag; 

//  the  angle  off  of  leg  one  where  the  leg  is  attachec  to 
//  the  body 

double  leg_attachment_angle; 

AquaLeg (AquarobotBodyS ,  double);  II  constructor  and  initializer 
-AquaLegO;  //  destructor 

void  Move Incremental (Aqua robot Body  fi,  double  deltal,  double  delta2, 
double  delta3) ; 

double  GetLegAttachment Angle ()  (  return  leg_attachment_angle; ) 

int  GetMotionCompleteFlagO  (  return  motion_complete_f lag; ) 

void  SetLegAttachmentAngle (double  angle)  ( leg_attachment  angle  -  angle 

void  SetMotionCompleteFlag(int  flag)  {motion_complete_f lag  -  flag;) 

int  GetLegSupportFlag ()  (  return  leg_support_f lag; ) 

void  SetLegSupportFlag (int  flag)  ( leg_support_f lag  -  flag;) 

)  ; 

#endif 


Figure  7.6 

C++  Code  Excerpt  Defining  Aquarobot 
Leg  Kinematics 


to  the  AquaLeg  class  are  included  within  the  class  definition. 


An  example,  shown  in  that  figure,  is  the  "Move  Incremental" 
function  which  increments  the  joint  angles  of  a  specified  leg 
by  a  given  amount. 

The  C++  function  similar  to  the  CLOS  "initialize-leg" 
function  is  the  C++  constructor  "AquaLeg"  shown  in  Figure  7.6. 
This  function  requires  an  AquarobotBody  class  and  a  double 
number  as  inputs.  This  and  the  other  AquaLeg  functions  are 
defined  within  the  AquaLeg. C  file  (Figure  7.7)  .  Like  the  CLOS 
version,  it  is  within  the  constructor  that  the  dependent 
objects,  LinkO  through  Link3,  are  instantiated.  Similar  to 
the  CLOS  version,  other  functions  may  be  called  or  slot  values 
altered.  The  "matrix"  class,  found  within  the  MatrixMy.C  and 
MatrixMy.H  files  (see  Appendix  C) ,  is  a  class  utility  and  is 
used  within  the  AquaLeg  constructor. 

D.  AQUAROBOT  OBJECT  INSTANTIATION  CODE 

Objects  may  be  constructed  in  various  composition  within 
an  OOP.  However,  the  method  of  actually  instantiating  an 
object  varies  among  00  languages.  The  CLOS  version,  shown  in 
Figure  7.4,  displays  one  top  level  object  while  the  C++ 
program,  shown  in  Figure  7.3,  makes  seven  subobjects  to 
produce  one  Aquarobot  system.  This  section  will  discuss  the 
individual  language's  method  of  instantiation  using  the  two 
Aquarobot  versions. 
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//  A********************************************************** 

//  FUNCTION:  -AquaLeg 0 

II  PURPOSE:  destructor  of  AquaLeg  class 

II  *********************************************************** 

AquaLeg : : -AquaLeg  < ) 

( 

delete  linkO; 
delete  linkl; 
delete  link2; 
delete  link3; 

) 

/  /  *********************************************************** 
//  FILENAME:  AquaLeg 

II  PURPOSE:  constructor  of  AquaLeg  class 
//  RETURNS:  AquaLeg  class  with  values 
//  ******..********************************.****************.* 

AquaLeg: : AquaLeg (AquarobotBody  Sbody,  double  angle) 

< 

motion_complete_f lag  ”1;  //  initializes  flag  value 

SetLegAttachmtntAngle (angle) ; 

linkO  -  new  LinkO; 

linkl  -  new  Linkl; 

link2  -  new  Link2; 

link3  «  new  Link'., 


//  initial  link  values  initialized 

//  temp  matrix  adds  in  the  T_matrix  needed  for  the  physical 
//  attachment  of  the  leg  to  the  body 
matrix  temp; 

II  updates  the  Transformation  matrix  from  body  center  to  the 
//  leg  attachment  point 

temp.UpdateTMatrix (Get LegAttachment Angle () , 0 . , 0 . , 0 . ) ; 
temp  «  ‘body . H_matrix  *  temp; 

linkO->RotateLink (Stemp  , linkO->GetInboardJointAngle () ) ; 

linkl->RotateLink (linkO->H_matrix,  linkl->GetInboardJointAngle  0 ) 
link2->RotateLink (linkl->H_matrix, link2->GetInboardJointAngle () ) ; 
link3->RotateLink (link2->H_matrix, link3->GetInboardJointAngle () ) ; 

) 


Figure  7.7 

C++  Code  Excerpt  Implementing  Aquarobot 
Leg  Kinematics 
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1.  CLOS  Ob j act  Description 

The  CLOS  version  produces  an  Aquarobot  by  performing 
the  function  "aqua-picture"  in  the  LISP  screen  environment. 
This  function's  code  is  displayed  in  Figure  7.8  and 
instantiates  a  top  level  object,  Aquarobot,  (named  "aqua-1") 
in  its  first  line  using  the  make-instance  command.  The  class 
"aquarobot"  is  used  as  the  blueprint  for  this  instantiation. 
This  class  consists  of  one  body  and  six  legs  ("legl"  through 
"leg6”)  as  dependent  objects.  These  slots  are  instantiated 
using  the  same  make-instance  command  when  an  "aquarobot"  is 
created.  Slot  values  are  instantiated  within  the  aqua-leg 
instantiation  using  the  variable  : lea-attachment-anqle  which 
was  the  initializing  argument  for  a  slot  with  the  same  name  in 
the  aqua-leg  class  (Figure  7.5). 

2.  C++  Object  Description 

Bot.C  (Figure  7.9)  is  the  main  program  the  C++ 
version.  This  program  controls  the  construction  of  the 
Aquarobot.  The  AquarobotBody  and  six  AquaLegs  are 
instantiated  at  this  top  level  and  they  are  all  subobjects 
since  there  is  no  explicit  Aquarobot  instantiated.  Similar  to 
CLOS,  each  object  is  given  a  name  (for  example  "leg3")  and 
initialization  values  at  the  same  time  it  is  instantiated. 
C++  does  not  provide  a  command  that  equates  to  CLOS's  make- 


(def class  aquarobot  O 
( (body 

rinitform  (make-instance  '  aquarobot -body) 

:  accessor  body) 

(legl 

:initform  (make-instance  'aqua-leg  : leg-attachment-angle  (deg-to-rad  0)) 

: accessor  legl) 

(leg2 

:initform  (make-instance  'aqua-leg  : leg-attachment-angle  (deg-to-rad  60)) 

:  accessor  leg2) 

(leg3 

:initform  (make-instance  'aqua-leg  : leg-attachment-angle  (deg-to-rad  120)) 
: accessor  leg3) 

(leg4 

:initform  (make-instance  'aqua-leg  : leg-attachment-angle  (deg-to-rad  180)) 
: accessor  leg4) 

(leg5 

:initform  (make-instance  'aqua-leg  : leg-attachment-angle  (deg-to-rad  240)) 
raccessor  legS) 

(leg6 

:initform  (make-instance  'aqua-leg  : leg-attachment -angle  (deg-to-rad  300)) 
raccessor  leg6) ) ) 

(defmethod  initialize  ((aqua  aquarobot)) 

(transform-node-list  (body  aqua)) 

(initialize-leg  (legl  aqua)  (body  aqua)) 

(initialize-leg  (leg2  aqua)  (body  aqua)) 

(initialize-leg  (leg3  aqua)  (body  aqua)) 

(initialize-leg  (leg4  aqua)  (body  aqua)) 

(initialize-leg  (legS  aqua)  (body  aqua)) 

(initialize-leg  (leg€  aqua)  (body  aqua))) 

(defun  aqua-picture  () 

(setf  aqua-1  (make-instance  'aquarobot)) 

(initialize  aqua-1) 

(setf  camera-1  (make-instance  'camera)) 

(create-camera-window  camera-1) 

(take-picture  camera-1  aqua-1) ) 


Figure  7.8 

CLOS  Code  For  Aquarobot  Class 
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4* 

f. 


main  O 

( 


/*  value  returned  from  the  event  queue  */ 

short  value; 
long  ma inmenu; 

long  hititem; 

FILE  *ifp; 

ifp  -  fopen ("bot .dat", "r") ; 

/*  initialize  the  IRIS  system  */ 
initialize () ; 

/*  Create  Pop  Op  Menus  * / 
ma  inmenu  -  makethemenus () ; 

//  make  the  robot  from  its  pieces 
AquarobotBody  aquabody; 

Aqua Leg  legl (aquabody, 0.0); 

Aqua Leg  leg2 (aquabody, 60.0); 

AquaLeg  leg3 (aquabody, 120.0) ; 

AquaLeg  leg4 (aquabody, 180 . 0) ; 

AquaLeg  leg5 (aquabody, 240.0) ; 

AquaLeg  leg6 (aquabody, 300 . 0) ; 


Figure  7.9 

C++  Code  Excerpt  From  Main  Program  Showing 
Instantiation  of  the  Parts  of  Aquarobot 
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instance  command.  Instead,  C++  instantiates  an  object  by 
declaring  the  class  and  providing  a  name  and  information 
necessary . 

E.  GRAPHICS 

1.  Graphics  Display 

The  CLOS  version  of  Aquarobot  (Appendix  A)  was 
graphically  simulated  on  a  low  end  Silicon  Graphics  Indigo 
graphics  workstation  using  a  LISP  camera  object.  This  is  the 
camera. cl  file  in  Appendix  A  and  it  was  created  as  a  debugging 
tool  because  CLOS  does  not  provide  a  graphics  capability 
within  its  library.  Examples  of  the  CLOS  graphics  produced  by 
a  camera  object  are  in  Appendix  C.  The  C++  version  of 
Aquarobot  (Appendix  B)  was  simulated  on  the  same  workstation 
as  the  CLOS  code.  The  C++  code  uses  the  system's  basic 
graphic  library,  gl.  The  C++  graphics  code  will  be  discussed 
in  this  section. 

The  C++  simulation  was  developed  to  support  the 
debugging  of  control  software.  A  user's  manual  explaining  its 
use  in  this  application  has  been  produced  (Suzuki,  1993)  .  The 
graphics  code  is  included  in  bot.C  in  Appendix  B.  This  file 
includes  the  main  program  (a  requirement  of  C++)  which 
provides  the  initial  instantiation  of  Aquarobot  and  controls 
function  calls.  This  file  also  provides  the  graphics  setup 
(in  the  initialize  function)  and  the  function  that  draws  the 


stick  figure  Aquarobot  (in  the  drawaqua  function).  This 
coordination  of  bot.C  is  depicted  in  the  flow  diagram  in 
Figure  7.10. 

Aquarobot  is  instantiated  in  the  reset  position. 
Figure  7.11  displays  this  first  graphical  view.  The  next 
motion  for  Aquarobot  is  provided  from  the  output  of  the  gait 
algorithm.  The  information  is  provided  by  the  position  and 
orientation  change  of  the  body  and  the  change  in  joint  angle 
for  each  joint  of  each  leg.  The  respective  changes  are 
transferred  to  the  body's  Movelncremental  function  and  leg 
Move  Incremental  function.  These  functions  update  desired  body 
and  link  positions.  The  body's  function  is  called  first 
because  each  leg  uses  the  body's  updated  H  matrix  computation 
in  their  functions.  The  function  FindPositions  determines  the 
Cartesian  coordinate  location  of  each  joint  and  the  footpad 
for  each  leg,  as  well  as  the  body's  position  and  orientation. 
Figure  7.12  shows  a  change  ordered  in  one  of  leg  one's  joint 
angles.  The  C++  graphics  code  continuously  polls  for  an 
acceptable  queued  signal.  This  signal  determines  the  path 
taken  and  the  functions  performed  within  that  option. 

The  CLOS  version  uses  the  user  interface  on  the 
terminal  as  its  main  program.  As  shown  in  the  script  file  of 
Appendix  C,  this  method  does  not  use  an  explicit  continuous 
polling  loop  like  C++.  Rather,  LISP  provides  an  infinite 
read-eval-print  loop  within  its  user  interface.  Appendix  C 
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Figure  7 . 10 

C++  Program  Flow  Diagram  for  Main  Program 
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also  provides  examples  of  Aquarobot  graphics  obtained  using 
the  code  in  Appendix  A. 


2 .  User  Interface 

In  the  C++  version  of  the  Aquarobot  simulation  code, 
one  acceptable  queue  signal  is  the  clicking  of  an  option  on 
the  menu  shown  upon  the  screen.  Figure  7.12  displays  the  menu 
and  its  options.  The  options  provide  various  camera  views  of 
Aquarobot  and  the  ability  to  read  from  a  data  file  that 
consists  of  data  changes.  The  camera  views  are  particularly 
helpful  in  the  debugging  of  gait  motions  conducted  in  the  gait 
algorithm  function. 

F.  SUMMARY 

C++  and  CLOS  are  both  similar  in  their  method  of  defining 
a  class.  The  instantiation  technique  differs  between  the 
languages  with  C++  requiring  a  class  constructor  function  that 
defines  the  instantiation  while  CLOS  uses  a  reserved  command 
and  the  class  definition. 

The  Aquarobot  programs  were  not  designed  from  identical  class 
and  object  hierarchies  and  this  provides  varied  examples  of 
design  as  well  as  00  language  variations.  Appendices  A  and  B 
include  the  entire  CLOS  and  C++  codes.  Appendix  C  provides  a 
script  file  and  graphical  pictures  produced  using  the  CLOS 
version.  The  next  chapter  evaluates  the  CLOS  and  C++  codes 
and  their  performance. 
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Figure  7.12 

Ordered  Motion  Aquarobot  Graphic  Display 


VIII.  EVALUATION 


» 


» 


A.  INTRODUCTION 

Both  the  CLOS  and  C++  versions  were  successful  in 
producing  a  graphic  simulator  of  Aquarobot.  Each  code, 
however,  varies  in  its  graphical  performance  and  code 
characteristics . 

B.  CLOS/ C++  CODE  EVALUATION 

The  CLOS  code  usually  requires  less  lines  of  code  to  write 
a  function  than  C++.  The  codes  in  Appendices  A  and  B  display 
approximately  a  three  to  one  ratio  of  length  in  favor  of  CLOS. 
This  compact  code  provides  CLOS  an  advantage  in 
understandability  and  prototyping.  Unfortunately,  CLOS 
function  definitions,  when  optimized  for  conciseness,  may  be 
rather  cryptic.  C++,  although  longer  in  length,  is  similar  in 
format  to  many  more  common  languages,  and  therefore  it  is  in 
some  ways  easier  to  read,  especially  by  programmers  not 
skilled  in  CLOS  and  LISP. 

CLOS  provides  dynamic  memory  allocation  and  uses  garbage 
collection  to  accumulate  unused  memory  space.  Activity  is 
suspended  during  garbage  collection  methods.  In  contrast,  C++ 
uses  a  memory  heap  which  requires  memory  to  be  removed  and 
then  explicitly  returned  to  the  heap  when  the  memory  space  is 
no  longer  needed. 
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C++  and  CLOS  are  both  similar  in  their  method  of  defining 
a  class.  The  instantiation  technique  differs  between  the 
languages  with  C++  requiring  a  class  constructor  function  that 
defines  the  instantiation  while  CLOS  uses  a  reserved  command, 
make- instance .  and  the  class  title. 

C.  CLOS/ C++  GRAPHICAL  EVALUATION 

Both  Aquarobot  versions  were  simulated  with  the  same  leg 
and  body  motions.  The  time  required  to  calculate  new  joint 
parameters  via  kinematics  was  recorded.  The  compiled  CLOS 
version  requires  310  ms  for  execution  and  display  of  one  move 
(six  degrees  of  freedom  for  body  motion  and  18  leg  joint 
motions)  while  160  ms  is  required  for  the  same  result  using 
the  C++  version. 

The  numerical  results  above  were  run  without  either  code 
being  optimized,  nor  were  compiler  switches  set  for  optimized 
code  generations.  The  C++  version  performed  faster  than  the 
CLOS  version  by  a  factor  of  two  to  one.  The  C++  speed 
advantage  was  not  noteworthy  enough  to  recommend  against  using 
CLOS  unless  execution  efficiency  is  the  dominant  factor.  In 
addition,  CLOS  required  less  programming  effort  (i.e.,  more 
compact  code)  than  C++.  This  author  recommends  CLOS  as  a 
prototype  executable  specification  code  when  a  complex  systems 
are  involved.  However,  the  common  format  and  familiarity  of 
C++  provides  a  better  comprehension  of  the  code  by  others  who 
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might  use  or  modify  it  (which  are  major  advantages)  .  In 
addition,  it  appears  that  C++'s  two  to  one  speed  ratio  may  be 
improved  with  optimization. 

D.  SUMMARY 

The  C++  and  CLOS  Aquarobot  objects  have  been  designed 
using  classes  that  are  common  to  all  rigid  body  manipulators. 
The  generic  classes  are  defined  in  both  code  versions  and  can 
be  incorporated  into  other  robot  design  code.  This 
reusability  of  00  code  as  well  as  its  simple  modification 
steps  are  advantages  to  this  design  in  this  and  other  research 
projects,  since  future  researchers  may  build  on  current  work. 
This  author's  C++  code  is  currently  being  used  in  Aquarobot 
dynamics  research  for  a  dissertation  at  Ohio  State  University 
(McMillan,  1992) . 

The  C++  code  is  also  being  used  for  its  original  purpose 
as  a  simulator  to  debug  control  algorithms.  Specifically, 
Master  degree  students  at  the  Naval  Postgraduate  School  are 
currently  using  this  simulator  to  investigate  gait  algorithms 
(Schue,  1993).  The  graphical  simulator  provides  a  useful  and 
time  saving  method  to  check  this  work  visually.  It  also 
provides  an  environment  where  innovative  methods  for  motion 
can  be  tried  without  requiring  the  physical  robot  to  be  put 
into  danger.  The  last  chapter  of  this  thesis  provides 
conclusions  concerning  this  research  project  and  suggests 
topics  for  future  research. 


IX.  CONCLUSIONS 


A.  INTRODUCTION 

This  thesis  represents  the  beginning  of  a  major  research 
effort  to  provide  a  generic  walking  robot  simulator,  although 
the  concepts  developed  here  can  be  used  for  any  articulated 
rigid  body  system  The  overall  research  project  is  centered 
around  Aquarobot,  a  six-legged  walking  machine  developed  in 
Japan.  While  the  techniques  and  the  computer  programs 
developed  and  used  here  are  only  a  small  part  of  the  overall 
effort  of  the  Naval  Postgraduate  School  Aquarobot  project, 
they  represent  critical  first  steps. 

B.  FUTURE  USE  OF  CODE  IN  OTHER  ROBOT  DESIGNS 

The  codes  discussed  in  this  thesis  and  included  in 
entirety  in  Appendices  A  and  B  provide  the  basic  classes 
necessary  to  create  any  rotary  link  manipulator  objects.  The 
C++  code  (Appendix  B)  provides  the  core  structure  that  any 
design  requires  to  produce  a  graphic  simulator  using  Silicon 
Graphics  Iris  systems.  The  CLOS  version  (Appendix  A)  includes 
a  camera  class  that  can  be  used  on  any  SUN  workstation  as  well 
as  any  Iris  system  which  has  been  provided  with  a  LISP 
compiler. 
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C.  FUTURE  USE  OF  AQUAROBOT 

Aquarobot's  underwater  walking  capability  provide  many 
possibilities  for  its  future  use.  Aside  from  its  original 
application  to  assist  with  the  quality  control  and 
construction  supervision  for  tsunamai  sea  wall  foundations, 
other  possibilities  include  its  use  at  marinas  to  locate  large 
items  dropped  into  the  water,  and  at  lakes  or  shallow  beaches 
when  an  underwater  search  is  needed.  Aquarobot  could  also  be 
helpful  in  detecting  underwater  cracks  in  piping  such  as 
electric  cables  and  gas  lines. 

Additionally,  the  Aquarobot  concept  presents  many  possible 
uses  in  military  applications.  For  example,  an  aquatic 
walking  machine  provides  an  alternative  method  of  mine 
detection  on  the  sea  floor  or  in  a  surf  zone.  Upon  completion 
of  an  analysis  of  the  feasibility  of  walking  between  land  and 
water,  it  may  be  found  that  this  class  of  robots  could  be  used 
for  beach  inspection  prior  to  an  amphibious  landing,  thus 
saving  lives. 

D.  FUTURE  RE8EARCH  IDEAS 

This  thesis  describes  the  initial  stage  of  the  Aquarobot 
research  project.  There  are  a  number  of  avenues  of  future 
reasearch.  Some  of  these  areas  include:  simulating  the 
robot's  dynamics,  modeling  the  joint  motors,  modeling 
Aquarobot's  hinged  foot  pads,  modeling  the  hydrodynamic 


effects  of  legged  walking  machines,  improving  the  graphic 
portion,  and  gait  planning  research. 

The  graphics  area  alone  provides  a  number  of  areas  of 
research  such  as:  providing  collision  detection  and  creating 
an  Aquarobot  replica  that  is  more  faithful  in  appearance  to 
the  actual  robot.  Both  graphics  codes  currently  display  a 
stick  figure. 

Aside  from  this  thesis'  use  of  CLOS  as  executable 
specification  code  for  a  C++  final  version,  CLOS  could  also  be 
used  in  incremental  development  of  C++.  This  would  take 
advantage  of  CLOS's  superior  debugging  environment.  Another 
alternative  in  code  production  would  be  to  use  CLOS  as  the 
main  program  and  import  C++  functions.  This  may  in  the  end 
prove  to  be  the  best  way  to  construct  an  interactive 
simulator,  but  this  has  yet  to  be  investigated. 

E.  SUMMARY 

Robotics  engineers  have  made  impressive  progress  in 
accomplishing  the  goal  of  producing  an  effective  and  practical 
walking  machine.  Such  a  machine  will  permit  society  to 
achieve  functions  and  carry  out  missions  not  presently 
possible.  Also,  they  will  improve  current  methods  of 
performing  tasks  we  do  now  but  not  very  efficiently,  or  with 
considerable  danger  to  human  workers. 

The  legged  robot  concept  is  in  its  infancy.  It  is  vital 
that  we  have  simulation  tools  capable  of  providing  an 
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accurate  model  of  complicated  linkage  mechanisms  used  for 
manipulation  and  locomotion.  These  tools  reduce  the  need  for 
a  prototype  vehicle  in  the  early  stages  of  development,  and 
provide  a  safe  environment  to  attempt  unusual  and  possibly 
dangerous  tests.  Trying  untested  gaits,  designs,  etc.  in  a 
simulation  environment  allows  for  better  understanding  of  the 
performance  envelope  and  capabilities  of  a  walking  machine, 
saves  money,  and  potentially  saves  lives. 

Simulation  tools  such  as  modeling  algorithms,  powerful 
computer  languages,  and  graphical  capabilities  enhance  and 
promote  technology.  There  are  many  ways  to  build  a  simulation 
model,  but  the  use  of  the  combination  of  kinematic  modeling, 
object  oriented  languages,  and  graphically  equipped  computer 
systems  offers  a  flexible  and  robust  design  method  for  both 
large  complex  robots  and  simple  one  limb  imitations  of 
organisms. 

Aquarobot  is  a  six-legged  walking  robot  whose  software 
will  be  improved  using  this  simulation  model.  Later 
generations  of  legged  machines  may  also  benefit  from  similar 
simulation  studies.  Aquarobot 's  actions  and  stability  in 

w 

various  postures  and  with  different  gaits  will  be  tested  to 
enhance  its  efficiency  and  design.  Hopefully,  through  these 
tests  and  improvements,  legged  machines  will  be  able  to 
exploit  their  many  advantages  in  areas  where  wheeled  vehicles 
are  now  used,  as  well  as  permitting  access  to  areas  where  no 
vehicle  can  now  go. 
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APPENDIX  A  -  CLOS  CODE 


link. el 

(defclaas  link  (rigid-body) 

( (mot  ion- 1 imit - f lag 
: in It  form  nil 

raccassor  mot  ion- limit -f lag) 
(twist-angle 
: initarg  : twiat-angla 
laceeaaor  twist -angle) 

(link-length 
: lnitarg  : link-length 
raccesaor  link-length) 

(inboard- joint -angle 
rinitarg  : inboard- joint -angle 
:acceiaac  Inboard- joint-angle) 
(Inboard- joint-displacement 
: initarg  : inboard- joint -displacement 
: accessor  inboard- joint -displacement ) 
( inboard- link 
: initarg  : inboard- link 
:aceaasor  inboard-link) 

(A-matrix 

:accessor  A-matrix))) 

(defclaas  rotary-link  (link) 

( (min-joint-angle 

: initarg  :min- joint-angle 
: accessor  min-joint-angle) 

(max- joint -angle 
: Initarg  :max- joint -angle 
:accessor  max- joint-angle) ) ) 

(defclaas  sliding-link  (link) 

( (min- joint -displacement 

: initarg  :min- joint -displacement 
:accessor  min- joint-displacement) 
(max- joint -displacement 
rinitarg  :max- joint-displacement 
taccessor  max- joint-displacement) ) ) 
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•qua -link. el 

(defclass  linkO  (rotary-link) 

( (twist -anqla  rinitform  0) 

(link-lanqth  :initforw  37.5) 

(inboard-joint-angle  rinitform  0) 

(inboard- joint -displacement  :initforai  0) 

(win- joint-angle  rinitform  (deq-to-rad  -360)1 
(wax- joint -anqla  rinitform  (deq-to-rad  380)) 

(noda-liat  :initfona  '((0  001)  (0001)  (-37.5  0  0  1))) 

(polygon- liat  rinitform  ‘(<1  3))))) 

(defclass  linkl  (rotary-link) 

( (twiat-angla  rinitform  (daq-to-rad  -90)) 

(link-lanqth  rinitform  30) 

( inboard- joint-angle  :lnitforw  0) 

( inboard- joint -displacement  rinitform  0) 

(min- joint -angle  :init(orw  (deq-to-rad  -60)) 

(max- joint-angle  rinitform  (deg-to-rad  60)) 

(node-list  rinitform  '((0  001)  (0001)  (-30  0  0  1))) 

(polygon- list  rinitform  ‘((1  3))))) 

(defclass  llnk2  (rotary-link) 

((twist-angle  :inlt(orm  0) 

(link-length  :initform  50) 

(inboard- joint-angle  :inittorm  (deg-to-rad  66.4)) 

( inboard- joint -displacement  :init(orm  0) 

(min- joint -angle  :initform  (deg-to-rad  -106.6)) 

(wax- joint-angle  rinitform  (deg-to-rad  73.4)) 

(node-list  rinitform  '((0  0  0  1)  (0  0  0  1)  (-50  0  0  1))) 
(polygon-list  rinitform  '((1  3))))) 

(dateless  link!  (rotary-link) 

( (twist -angle  rinitform  0) 

(link-length  rinitform  100) 

(inboard- joint -angle  rinitform  (deg-to-rad  -156.4)) 

( inboard- joint -displacement  :initform  0) 

(min- joint-angle  :init(orm  (deg-to-rad  -156.4)) 

(max- joint -angle  rinitform  (deg-to-rad  33.6)1 
(node-list  :initform  M(0  0  0  1)  (0  0  0  1)  (-100  0  0  1))) 
(polygon-list  rinitform  '((1  3))))) 

(defwethod  update-A-matrix  ((link  link)) 

(with-slots  (twist-angle  link-length  inboard- joint -angle 
inboard- joint -displacement  k-watrix)  link 
(sett  A-matrix  (dh-matrix  (cos  inboard- joint -angle) 

(sin  inboard- joint -angle)  (cos  twist-angle)  (sin  twist-angle) 
link-length  Inboard- joint-displacement) ) ) ) 

(defun  deg-to-rad  (angle)  (*  .017453393519943395  angle)) 

(defwethod  rotate  ((link  rotary-link)  angle) 

(sett  ( inboard- joint-angle  link)  angle) 

(update-A-matrix  link) 

(sett  (H-matrix  link)  (matrix-multiply  (H-matrix  (inboard-link  link)) 

(A-metrix  link))) 

(transform-node-list  link)) 

(defwethod  rotate-link  ((link  rotary-link)  angle) 

(cond  ((>  angle  (max- joint -angle  link)) 

(rotate  link  (wax- joint-angle  link)) 
(setf  (mot ion-limit-f lag  link)  t)) 
((<  angle  (min- joint -angle  link)) 

(rotate  link  (win- joint-angle  link)) 
(setf  (motion-limit-flag  link)  t ) ) 

(t  (rotate  link  angle)  (setf  (motion-limit-f lag  link)  nil)))) 
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iqut'lafi.el 


1 


(daf class  aqua -1«Q  () 

( ( lag-attachaiant-angla 

:initarg  :  lsg-attach»snt -angla 
:acc«fior  lag-attaehaant-angla) 

(linkO 

:initfora  (naka-inatanca  'llnkO) 

:accanor  llnkO) 

(linkl 

:initfom  (naka-inatanca  * linkl) 

:accaasor  linkl) 

( link2 

:initfora  (naka-inatanca  ’llnk2) 

:accaaaer  link2) 

(link! 

:initform  (naka-inatanca  ‘link!) 

:aceaaaor  linkl) 

(not ion -coaplata- flag 
:init(orn  nil 

:accaaaor  motion-complata-f lag) 

( previous -  foot -poa 1 1 ion 
:init(orn  nil 

: accessor  pravioua -foot -poa it ion) 

( currant  -  foot  -poa  it  ion 
rinitfom  nil 

:accaaaor  currant-foot-poaition) ) ) 

(da (met hod  initialize-leg  ((lag  aqua-lag)  (body  aquarobot-body) ) 

(aatf  (inboard-link  (linkO  lag))  body) 

(aatt  (inboard-link  (linkl  lag))  (llnkO  lag)) 

(aatf  (inboard-link  (link2  lag))  (linkl  lag)) 

(aatf  (inboard-link  (link!  lag))  (llnk2  lag)) 

(rotata-llnk  (linkO  lag)  ( lag-attachnant-angla  lag)) 

(rotata-link  (linkl  lag)  (inboard- joint -angla  (linkl  lag))) 

(rotata-link  (link2  lag)  (inboard- joint -angla  (linkl  lag))) 

(rotata-link  (link3  lag)  ( Inboard- joint-angla  (linkl  lag))) 

(aatf  (currant-foot-poaition  lag) 

(near  3  Iflrat  (tranafonaad-noda-liat  (link!  lag)))))) 

(dafawthod  taka-plctura  (Ictaara  eaiaara)  (lag  aqua-lag)) 

(taka-pictura  caaara  (linkl  lag)) 

(taka-plctura  caaara  (link2  lag)) 

(taka-plctura  caaara  (linkl  lag))) 

(dafaathod  aova-incraaantal  (( lag  aqua-lag)  lncreaant-llst) 

(rotata-link  (llnkO  lag)  (lag-attachaant-angla  lag)) 

(rotata-link  (linkl  lag) 

(♦  (first  incranant-liat)  (inboard- joint -angla  (linkl  lag)))) 
(rotata-link  (link2  lag) 

(♦  (sacond  incraaant-llst )  ( inboard- joint-angla  (llnk2  lag)))) 
(rotata-link  (linkl  log) 

(♦  (third  incroaant-list)  (inboard- joint-angla  (linkl  lag)))) 
(satf  (pravious-foot -position  lag)  (currant-foot -position  lag)) 

(sotf  (currant-foot -position  lag) 

(near  3  (first  (transfonaad-nods-list  (link!  lag))))) 

(aatf  (a>ot ion-coaplata- flag  log)  (not  (or  (notion -Halt -flag  (linkl  log)) 

(sntion-liait-flag  (link!  lag))  (sot ion- linit- flag  (linkl  lag)))))) 

(dafaathod  f eas ible-aovep  ((lag  aqua-lag)  allowabla-slnkaga  allowablo-allppaga) 
(and  (<•  (third  (currant-foot-poaition  log))  allowablo-sinkago) 

(or  (alnusp  (third  (eurront-foot-position  log))) 

(ainusp  (third  (pravious-foot -position  log))) 

(<•  (vactor-langth  (vactor-slippago  lag))  allowabla-slippaga) ) ) ) 

(dafaathod  vactor-slippago  ((lag  aqua-log)) 

(vactor -subtract  (rast  (ravarsa  (pravious-foot -position  lag))) 

(rast  (ravaraa  (currant-foot -position  lag))))) 
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(dafclass  aqua robot -body  (rigid-body) 

( (noda-lisc 

: initform  '((9  001)  (37. 5  001)  (1S.7S  32.48  0  1) 

(-18.75  32.48  0  1)  (-37.5  001)  (-18.75  -32.48  0  1) 

(18.75  -32.48  0  1)  (37.5  0  -15  1))) 

(polygon-llat 

: initform  -((1  2  3  4  5  S)  (1  7))) 

(H -matrix 

.'initform  (homoganaoua-transform  0  0  0  0  0  z-init)))) 

(date lass  aquarobot  () 

( (body 

: initform  (make-instance  'aquarobot -body ) 

:accassor  body) 

(lagl 

.-initform  (maka-instanca  'aqua-lag  : leg-attachment -angle  (dag-to-rad  0)) 
.■aecassor  lagl) 

<lag2 

:initform  (maka-instanca  'aqua-lag  : lag-attachmant -angla  (dag-to-rad  80)) 
:ac cassor  lag2) 

(lagl 

:inltform  (maka-instanca  'aqua-lag  : leg-attachment -angla  (dag-to-rad  120)) 
:accassor  lagl) 

(lag4 

tinitform  (maka-instanca  'aqua-lag  : leg-attachment-angle  (dag-to-rad  180)) 
: aecassor  lag4) 

(lags 

:initfora  (maka-instanca  'aqua-lag  : lag-attachmant -angla  (dag-to-rad  240)) 
; aecassor  lagS) 

(lag6 

: initform  (maka-instanca  'aqua-lag  : lag-attachmant -angla  (dag-to-rad  300)) 
: aecassor  lagS))) 

(dafmathod  inieializa  ((aqua  aquarobot)) 

(transform-noda-list  (body  aqua)) 

( lnleializa-lag  (lagl  aqua)  (body  aqua)) 

( lnitializa-lag  (lag2  aqua)  (body  aqua)) 

( initializa-lag  (lagl  aqua)  (body  aqua)) 

(initlaliza-lag  (lag4  aqua)  (body  aqua)) 

(initializa-lag  (lagS  aqua)  (body  aqua)) 

(lnitializa-lag  (lagS  aqua)  (body  aqua))) 

(dafun  aqua-pictura  () 

(satf  aqua-1  (maka-instanca  'aquarobot)) 

(initialize  aqua-1) 

(satf  camera -1  (maka-instanca  ’eamara)) 

(create-camera-windoe  eamara- 1) 

(taka-pictura  camera- 1  aqua-1)) 

(dafmathod  take-plctura  ((camera  camera)  (aqua  aquarobot)) 

(take-picture  camera  (body  aqua)) 

(take-picture  camera  (lagl  aqua)) 

(take-plctura  camera  (lag2  aqua)) 

(take-picture  camera  (leg3  aqua)) 

(take-picture  camera  (leg4  aqua)) 

(take-picture  camera  (leg5  aqua)) 

(taka-picture  camera  (lagS  aqua))) 

(dafun  new-plctura  () 

(taka-picture  camera-1  aqua-1)) 

(dafeonstant  z-init  -54.1818SS) 

(dafmathod  mova-incramantal  ((aqua  aquarobot)  increment-list) 


aqua. el 


a 


(mova-incremantal  (body  aqua)  (first  incramant-list) ) 

(move- incramsntal  (lag!  aqua)  (aacond  increment-list) ) 
(move-incremental  (lag?  aqua)  (third  increment -list) ) 

(move- incraaiantal  ( leg3  aqua)  (fourtn  increment -list  I  ) 
(■ova-lncraaMntal  (lags  aqua)  (fifth  increment-list)) 

(move-incremental  (lagS  aqua)  laixth  increment -list) ) 

(move-incremental  (lags  aqua)  (savanth  Incramant-list) ) ) 

(dafconstant  null-move-list  '((9  0  0  0  0  0)  (000)  (000)  (000) 

(0  0  0)  (0  0  0)  (0  0  0))) 

(dafmathod  faasibla-movap  ((aqua  aquarobot)  allowable-stnkage 

allowable-slippage) 

(and  ( faasibla-movap  (lagl  aqua)  allowabla-ainkaga  allowabla-sllppaga) 

( faasibla-movap  (lag2  aqua)  allowabla-ainkaga  allowabla-sllppaga) 

( faasibla-movap  (lagl  aqua)  allowabla-ainkaga  allowabla-sllppaga) 

( faasibla-movap  (lags  aqua)  allowabla-ainkaga  allowabla-slippaga) 

( faasibla-movap  (lagS  aqua)  allowabla-sinkaga  allowabla-slippaga) 

( faasibla-movap  (lags  aqua)  allowabla-ainkaga  allowabla-slippaga))) 
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camara. cl 


1 


(require  :xcv) 

( cw : in i t ia 1 i za - common -window* ) 

(dateless  caawra  (rigid-body) 

( (foeal-langth 

: accessor  focal -length 
:lnitform  S) 

(pravioua -point 
taccasaor  pravioua -po int ) 

(camera -window 
taccasaor  camara -window) 

(H-matrix 

tinitform  (homoganaous-tranafonn  .3  -.3  0  -300  -90  -90)) 

( invar aa-H-matrix 
taccasaor  invarsa-H-matrix 

tinitform  '  nverse-H  (homogeneous-transform  .3  -.3  0  -300  -90  -90))) 
(anlargataant-factor 
taccasaor  enlargement -factor 
t initform  30) ) ) 

(da f mat hod  creata-eamera-window  ((camara  camara)) 

(aatf  (eamara-window  camara) 

(cwimake-window-straam  tbordars  5 
t left  500 
: bottom  SOO 
twidth  300 
.•height  300 
ttitle  • aqua robot • 
tactivata-p  t) ) ) 

(dafmathod  nova  ((camera  camara)  azimuth  elevation  roll  x  y  z) 

(aatf  (H-matrix  camera)  (homoganaous-tranafonn  azimuth  elevation  roll  x  y  z) ) 
(aatf  (invaraa-H-maeTix  camera)  (invaraa-H  (H-matrix  camara) )) ) 

(dafmathod  take-picture  ( (csmera  camara)  (body  rigid-body)) 

(doliat  (polygon  (polygon-list  body)) 

(draw-polygon  camera  polygon  (traneformad-noda-list  body)))) 

(dafmathod  draw-polygon  ((camera  camera)  polygon  node-coord- Hat) 

(let*  ( (atarting-noda- index  (first  polygon)) 

(ramaining-node-indices  (rest  polygon)) 

(atart-polnt-coord  (nth  starting-node- index  node-coord- list) ) ) 
(transform-and-mova-pan-to  camara  atart-polnt-coord) 

(doliat  (noda-indax  remaining-node-indlces) 

(trunsform-and-draw-to  camara  (nth  noda-indax  noda-coord-liat) ) ) 
(transform-and-draw-to  camara  atart-point-coord) ) )  , -closes  polygon 

(dafmathod  transform-and-mova-pan-to  ((camera  camera)  point-in-aarth-apace) 
(aatf  (previous-point  camara) 

(eomputa-camara-vlndow-coordinatas  camera  point-ln-aarth-apaca) ) ) 

(dafmathod  transform-and-draw-to  ((camara  caawra)  point -in-aarth-spaca) 

(let  ( (to-point 

(compute -camara -window-coordinates  camara  point- ln-aarth-epaca) ) ) 
(draw-llne-ln-eamera-window  camara  (previous-point  camara)  to-polnt) 

(aatf  (previous-point  camara)  to-point))) 

(dafmathod  draw-lina-in-camera-window  ((camera  caawra)  start  and) 

(cw:draw-iina  (camera-window  camara) 

(cwtmaka-poaition  :x  (first  start)  :y  (second  start)) 
(cwtmaka-positlon  :x  (first  and)  :y  (second  and)) 

: brush-width  0)) 

(dafmathod  compute-camera-window-eoordinates  ((camara  camera) 
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point-in-aarth-apaca) 

(lat*  ( (anlargaawnt -factor  ( an largamant- factor  coin)) 

(  focal -length  (focal-langth  ciMtil! 

(point - in-camara-spaca  (poat -Multiply  ( invaraa-H-awtrlx  caawra) 

point-in-aarth-apaca) ) 

(x  (firat  point-ln-caiaara-apaca) )  ;x  axla  la  along  optical  axia 
(y  (aacond  point - in-caawra-apaea) )  ;y  ia  out  right  aida  ot  caaara 
(z  (third  point-in-caaara-spaca)  M  ;z  ia  out  bottoa  of  camara 


(if  (>•  x  focal-langth)  ;  handlaa  raar  clipping 

(liat  (♦  (round  (*  anlargaaant- (actor  (/  (*  (ocal-langth  y)  x) ) ) 

ISO)  ; to  right  in  caawra  window 

(♦  150  (round  (♦  anlargaawnt -factor  1/  (*  focal-langth  (-  z ) )  x))))) 

;up  in  caawra  window 


(liat  -1  -DM) 
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load-f  Uaa  .el 

(load  'camera .cl  ‘ ) 

(load  *link.cl*| 

(load  ‘rigid-body . cl " ) 

(load  'robot-kinaaatica.el') 
(load  ‘aqua. cl*) 

(load  * aqua- leg. cl*) 

(load  ‘aqua-link. cl*) 
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rigid-body. el 

(defclass  rigid-body 
() 

( ( location 

:initarg  -.location 
: accessor  location) 

(velocity 

rinitarg  : velocity 
:accessor  velocity) 

(acceleration 
:acceeaor  acceleration) 

( (orcea -and- torques 
•-accessor  forces-and-torques) 
(lacsaents -of  -  inert  la 
ilnltarg  rmoments-of -inert  la 
:  accessor  moments-of-inertia) 
(awes 

:initarg  :naas 
: accessor  mass) 

(node-list 
:initarg  inode-list 
laccasaor  node-list) 
(polygon-list 
:initarg  :polygon-list 
: accessor  polygon-list) 
(tranafonaed-node-liat 
: accessor  transfonaed-node-list) 
(H-iaatri* 

: accessor  H-matrix) 

(current-time 
: accessor  current-time))) 


(defmethod  move  ((body  rigid-body)  azimuth  elevation  roll  x  y  z) 
(setf  (H-matrix  body) 

(homogeneous-transform  azimuth  elevation  roll  x  y  zll 
(transform-node-list  body) 

(update-position  body)) 


(defmethod  move-incremental 
(setf  (H-matrix  body) 

(matrix-mult iply  (H-aiatrix  body) 


(transform-node-list  body) 
(update-position  body)) 


((body  rigid-body)  increment-list) 


( homogeneous - t rans form 
(first  increment -list) 
(second  increment -list) 
(third  increment-list) 
(fourth  increment-list) 
(fifth  increment -list) 
(sixth  increment  - 1 ist) ))  | 


I 


I 


I 


(defmethod  get -delta-t  ((body  rigid-body)) 

(let*  ((new-time  (get-internal-real-time)) 

(delta-t  (/  (-  new-time  (current-time  body))  1000))) 
(setf  (current-time  body)  new-time) 
delta-t) ) 

(defmethod  start -timer  ((body  rigid-body)) 

(setf  (current-time  body)  (get-internal-real-time))) 

(defmethod  update-rlgid-body  ((body  rigid-body)) 

(let  ((delta-t  (get -delta-t  body))) 

(update-acceleration  body) 

(update-H-matrix  body  delta-t) 

(transform-node-list  body) 
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(update-position  body) 

(update-velocity  body  delta-t))) 

(defiaethod  updata-accalaration  ((body  rigid-body)) 

(aaef  (accalaradon  body)  ;;(ll*t  u-dot  v-dot  w-doe  p-doc  q-dot  r-dot) 

(mult iple- value-bind 

(Fx  Fy  Fr  t.  H  N  u  v  *t  p  q  r  Xx  Iy  ID 
(values- List 
(append 

( forces-and-torquea  body)  (velocity  body)  (moments-of-inertia  body))) 


(‘ 

(* 

V  r) 

(*  -1  w  q)  (/  Fx  (maaa  body)) 

(* 

•gravity  (first  (third  (H-matrix  body) ))) ) 

(* 

(* 

w  P) 

(*  -1  u  r)  (/  Fy  (maaa  body)) 

(* 

•gravity*  (second  (third  ( H-matrix  body) ))) ) 

(♦ 

<* 

u  q) 

(*  -1  v  p)  (/  Fr  (maaa  body)) 

(* 

•gravity*  (third  (third  (H-matrix  body )))) ) 

(/ 

<* 

(*  (- 

Iy  ID  q  r)  L>  lx) 

(/ 

(» 

(*  (- 

It  lx)  r  p)  M)  Iy) 

(/ 

(* 

C  (- 

lx  Iy)  P  q)  H)  ID  )  ) ) ) 

(defiaethod  updata-valocity  ((body  rigid-body)  dalta-t) 

(satf  (velocity  body) 

(vactor-add  (velocity  body) 

(•e*l«r-«ultiply  dalta-t  (acceleration  body))))) 

(defiaethod  updata-H-i»atrix  ((body  rigid-body)  dalta-t) 

(eetf  (H-matrix  body) 

(matrix-mult  ip ly 

(H-matrix  body)  (homogeneous-transform 

(*  dalta-t  (aixth  (velocity  body))) 

(*  dalta-t  (fifth  (velocity  body))) 

<*  dalta-t  (fourth  (velocity  body))) 

(*  dalta-t  (firat  (velocity  body))) 

'.*  dalta-t  (aecond  (velocity  body))) 

(*  dalta-t  (third  (velocity  body))))))) 

(dafmethod  tranaform-noda-liat  ((body  rigid-body)) 

(aatf  ( trans f ormed-noda- l iat  body) 

(mapcar  #• (lambda  (node-location) 

(poat-aailtlply  (H-matrix  body)  node- locat ion ) ) 
(node-llat  body)))) 

(defiaethod  update-poeition  ((body  rigid-body)) 

(••tf  (location  body)  (near  J  (firat  (tranaformed-node-llet  body))))) 

(defiaethod  get-node-polygon-llat  ((body  rigid-body)) 

(Hat  ( t rana formed -node -1  iat  body)  (polygon-lire  body))) 

(defconatant  ‘gravity*  32.2185) 
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robot -kinaaatiee  .el  1 

(defun  transpose  (A) 

(cond  ((null  (cdr  A))  (mapcar  ‘list  (car  A))) 

(t  (mapcar  'cons  (car  A)  (transpose  (cdr  A)))))) 

(defun  dot -product  (x  y)  ;A  vecto-  is  e  list  of  numerical  atoms. 

(apply  '*  (mapcar  ' *  x  y ) ) )  ;A  matrix  is  a  list  of  row  lists. 

(defun  vector-length  (x)  (sqrt  (dot-product  x  x I > ) 

(defun  post-multiply  (M  x) 

(cond  ((null  (cdr  Ml)  (list  (dot -product  (car  Ml  x) I ) 

(t  (cons  (dot-product  (car  Ml  xl  (post-multiply  (cdr  Ml  x) ) 1 1 ) 

(defun  pre-multiply  (x  Ml 

I  post -multiply  (transpose  Ml  xl I 

(defun  matrix-multiply  (A  B) 

(cond  ((null  (cdr  All  (list  (pre-multiply  (car  A)  Bill 

(t  (cons  (pre-multiply  (ear  A1  Bl  (matrix-mult iply  (cdr  Al  Bill)) 

(defun  chain-multiply  (LI 

(cond  ((null  (cddr  LI  I  (matrix-multiply  (aval  (car  LI)  (aval  (cadr  LI) 1 1 
(t  (matrix-multiply  (aval  (car  LI  I  (chain-multiply  (cdr  Lilli)) 

(defun  cycle-left  (L)  (mapcar  ' row-cycle-left  L) I 

(defun  row-cycle-left  (R)  (append  (cJr  R)  (list  (car  RID) 

(defun  cycle-up  (Ml  (append  (cdr  Ml  (list  (car  Ml  1 1 1 

(dafun  uni  -vector  (one-column  length) 

(do  {(n  length  (1-  nil 

(R  nil  (cons  (cond  ((»  one-column  n)  1)  (t  0)1  Rll) 

( (zerop  n)  R) I  I 

(defun  unit-matrix  (n) 

(do  ((row-number  n  (1-  row-number)  I 

(I  nil  (cons  (unit-vector  row-number  n)  III) 

((zerop  row-number)  Illl 

(defun  concat-matrix  (A  B) 

(cond  ( (null  A)  B) 

(t  (cons  (append  (car  A)  (car  B) I  (concat-matrix  (cdr  A)  (cdr  B) I  1 1 1 1 

(defun  augment  (A)  (concat-matrix  A  (unit-matrix  (length  A) 1 1  I 

(detun  normalize-row  (R)  ( scalar-mult iply  (/  1.0  (car  Rll  Rll 

(defun  scalar-multiply  (a  x) 

(cond  ((null  x)  nil) 

(t  (cons  (*  a  (car  x) I  (scalar-multiply  a  (cdr  x 1 1 1  1 1 ) 

(defun  solve-first-column  (Ml 
(do*  ( (LI  M  (cdr  LI) I 

(L2  (normalize-row  (car  Mil) 

(L3  (list  L2)  (cons  (vector-add  (car  LI) 

(scalar-multiply  (-  (caar  LI) I  L2I)  L3II) 

((null  (cdr  LI) I  (reverse  L3III) 

(defun  vector-edd  (x  y)  (mapcar  '♦  x  y) I 
(defun  vector-subtraet  (x  y)  (mapcar  '-  x  y) I 
(defun  square-ear  (Ml 


I 


» 


» 


I 


» 


> 


I 


» 


» 


» 
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(do  ((n  ( length  Ml) 

(LI  M  (edr  LD) 

(U  nil  (cona  (near  m  (ear  Ll)>  L2))) 

((null  LI)  (reverse  L2)))) 

(da fun  nedr  (n  L)  (eond  ((serop  n)  L)  (t  (edr  (nedr  II-  n)  L) ) ) ) ) 

(da fun  near  (n  L)  (eond  ((serop  n)  nil) 

(t  (cona  (ear  L)  (near  (1-  n)  (edr  L) ) ) ) ) ) 

(dafun  naax-car-f irat  (n  L) 

( append  (auut-car-f  irat  (near  n  L) )  (nedr  n  L) ) ) 

(dafun  aMtrix- inverse  (M) 

(do  ((Ml  (SMX-ear-f irat  (augnent  M) ) 

(eond  ((null  Ml)  nil) 

(t  (wax-car- first  n  (cycle- left  (cycle-up  Ml) ) ) ) ) ) 

(n  (1-  (length  M) )  (1-  n) ) ) 

((or  (aiinuap  n)  (null  Ml))  (eond  ((null  Ml)  nil)  (t  (square-car  Ml)))) 
(setq  Ml  (eond  ((larop  (eaar  Ml))  nil)  (t  (solve-f irst-coluan  Ml)))))) 

(dafun  aMX-ear-f irat  (L) 

(eond  ((null  (edr  L))  L) 

(t  (if  (>  (aba  (eaar  L) )  (aba  (eaar  (aMX-car-f  irat  (edr  L) ) ) > )  L 
(append  (Max-ear -f Irak  (edr  LI)  (liat  (car  L) ) ) ) ) ) ) 

(dafun  dh -Matrix  (eoarotate  ainretata  eoetvlat  » in twist  length  translate) 
(list  (list  eoarotate  (-  (*  coatwist  s Inrotate)) 

<*  sintwist  sinrotata)  (*  length  eoarotate)) 

(Hat  sinrotata  <*  coatwist  eoaroeata) 

(-  (•  sintwist  eoarotate))  (•  length  sinrotata)) 

(list  0.  sintwist  coatwist  translate)  (liat  0.  0.  o.  1.))) 

(dafun  hoMoganaous-transforw  (atlnuth  elevation  roll  x  y  a) 

(rotation -and- translation  (sin  aaisaath)  (cos  axlnuth)  (ain  elevation) 

(coo  elevation)  (sin  roll)  (coo  roll)  x  y  D) 

(defun  rotat ion-and-t ranslat ion  ( spal  epel  sth  cth  sphi  ephi  x  y  a) 

(Hat  (liat  (*  epel  cth)  (-  (•  cpsi  ath  sphi)  (*  apei  ephi)) 

(♦  (*  cpal  ath  ephi)  (♦  apei  sphi))  x) 

(liat  (•  apai  cth)  (•  (*  epal  ephi)  (•  spal  sth  sphi)) 

(-  (•  spat  ath  ephi)  (*  epel  sphi))  y) 

(list  (-  sth)  (•  eth  sphi)  I*  eth  ephi)  D 
(list  0.  0.  0.  1.))) 

(defun  inverae-H  (H) 

(let*  ((Minus-?  (list  (-  (fourth  (first  ?))) 

(-  (fourth  (second  H))) 

(-  (fourth  (third  K)>))) 

(inverse-?  (transpose  ( square-car  (reverse  (rest  (reverse  ■)))))) 
(inverse-?  (post -Mult iply  inverse-?  Minus-?) ) ) 

(append  (coneat -Matrix  inverse-?  (transpose  (list  Inverse-?) I ) 

(list  (list  0  0  0  1))))) 
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bot.h 


1 


//  . . . . . 

//  FILENAME:  bot.h 

//  PURPOSE:  defines  constants  and  functions  usad  int  bot.C 
II  NOTE:  This  is  an  IRIS  30  program  written  in  C++ 

//  AUTHOR:  S  L  Davidson 
II  DATE:  S  January  1H3 

II  . . . . . . . . . 


I 


II  provides  constants  for  stenu  processing  options 

•define  CAMERA  1 

•define  above  2 

•define  BEHIND  3 

•define  RTS IDE  4 

•define  LTSIDE  S 

•define  TILEREAD  S 

•define  KETBDREAD  1 

•define  RESETriLE  B 

•define  RESET  14 

•define  EXIT  IS 


t 


» 


•  define  NEARCLIPPING  10.0  //  planes  defined 

•define  F ARCLIPPING  1023.0 


•define  vienx  -  0.0  /*  location  of  the  viewooint  */ 

•define  VIEWY  -  40.0 
•define  VIEWZ  -  400.0 


•define  REFX  -  0.0  /*  location  of  the  robot  ♦/ 

•define  REFT  -  0.0 
•define  REFZ  -  -200.0 


long  maketheinamaa  () ; 


static  float  rfx;/* 
static  float  rfy:/+ 
static  float  rfx.-/* 


reference  point  on 
reference  point  on 
reference  point  on 


in  the  a  direction  */ 
in  the  y  direction  */ 
in  the  x  direction  */ 


static  float  vx.-  /•  view  point  on  in  the  x  direction  •/ 
static  float  vy;  /«  view  point  on  in  the  y  direction  •/ 
static  float  vx;  /♦  view  point  on  in  the  x  direction  */ 


double  deltal,delta2, delta 3; 

double  delex,delel,delrol,delx,dely,delx; 


void  processnenuhit (long  hitltem) ; 

void  initialised;  //  initialixes  graphiea  layout 

void  loadunitl);  II  a  unit  matrix  used  in  rotation/translation 

void  projectionandviewingmatrix(float  vx, float  vy, float  vs, float  refx, float  refy, float  ref 

t); 

void  buildnonmovingvlewingxiatrixf  float  vx, float  vy, float  vx,  float  refx, float  refy.float  re 
fxl; 

void  drawagua (double*,  double*,  double  *,  double  *,  double  *, 
double  *,  double  *);  //  Includes  all  legs  and  body 


I 


I 


» 


> 


» 
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bot.C 


1 


» 


» 


. . * . . . * . 

II  FILENAME:  bot.C 

//  PURPOSE:  This  file  Mku  a  atick  aqua  robot  graphics 
//  interactive  design 

II  It  utilicea  Kineswtic  functions  to  detenaine  xy t 

//  coordinatas 

//  CONTAINS:  functions  shown  in  bot.h 

II  NOTE:  This  is  and  IR15  3D  program  written  in  c*4 

II  AUTHOR:  S  L  Davidson 
//  DATE:  IS  February  1»»3 
//  . . . . . 

•include  “gl.h*  II  graphics  library 
•include  “device. h“  //  graphics  librsry  file 
•include  “bot.h“  //  declaration  file 
•include  <atdio.h>  //  C++  library 
•Include  “Link.H* 

•include  “RigidBody .H* 

•  include  “MatrixMy.il* 

•Include  "Aqua robot Body .  H" 

•include  “Kinasntica.C* 

•include  “AquaLeg.H* 

mainO 

( 

//  value  returned  froaa  the  event  queue 
short  value; 
long  aialraaenu; 
long  hltltem.- 

FILE  +ifp; 

ifp  «  fopen (“bot . dat“, *r“) ; 

II  initialise  the  IRIS  system 
initialised  ; 


//  Create  Pop  Up  Menus 
■uimaanu  -  makethemenus O  ; 

//  makes  the  robot  from  its  pieces 
Aqua robot Body  aquabody; 

AquaLeg  legl (aqua body, 0 . 0) ; 

Aqua Lag  leg2 (aquabody , (0.0); 

AquaLag  leg3 (aquabody, 120 . 0) ; 

Aqua Lag  leg* (aquabody, 1(0 . 0) ; 

Aqua Leg  legs (aquabody, 240. 0) ; 

Aqua Leg  leg( (aquabody, 300 . 0) ; 

Aeturn_Coordinates  coord; 

Passing_lte*s  pass; 

Next_Motion  trans ; 
int  ans; 
ans  -  0; 

paaa.legnum  -  9; 

coord  -  rindPosit ions (aquabody,  legl,  leg2,  leg3,  leg*,  legS,  leg(); 
trans  -  TransferToCait (coord,  aquabody); 

while (TRUE) 


» 


> 


> 


► 


> 


\ 


» 
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I 

//  do  w  haw  aomathing  on  tha  awnt  quaua? 
if  (qteat  () ) 

I 


switch (qraad l(valua) ) 

( 

eat*  UDIUW: 

raahapaviawport  (I ; 
braak; 

eaaa  MBMimuTTOM: 

if  (value  —  1) 

I 

hit item  -  dopup (nainmenu) ; 
proceaamenuhlt  (hititaai) ; 

I 

braak; 

//  queue  uaad  for  calling  tha  gait  algorithai 
case  AKEY: 

trana  -  Gait  Algorlthm(trana) ; 

dalax  •  trana. bodymetion 10) ; 
dalal  »  trana. bodymot ion  Ill r 
dalrol  ■  trana. bodyantion |2); 
dala  -  trana. bodyaiotion 1 3]  : 
daly  -  trana. bodymot ion  ( 4 ] ; 
dala  «  trana. bodymot ion  13); 

aquabody  .Howlncramantal  (dalax, dalal. dolrel, dala, daly, dalx) ; 
lagl .  Howlncramantal  ( aquabody ,  t  rana .  laglmot  ion  1 0 ) , 
trana . laglmot ion ( 1) , trana . laglmot ion (2 ) ) ; 

lag2  .Howlncramantal  (aquabody.  trana .  Iag2motion  10) , 
trana . lag2mot ion ( 1 ) , t  rana . lag2motion (2) ) ; 

lag3  .Howlncramantal  (aquabody,  trana .  lagSmotion  (0) , 
trana . laglmot ion ( 1 ) .trana . laglmot ion (2) ) ; 

lagf  .Howlncramantal  (aquabody,  trana .  laglmot  ion  (0) , 
trana . lag (mot  ion ( 1 ) , trana . logamotion (2) ) ; 

lags  .Howlncramantal  (aquabody,  trana .  legSmotlon  (0) , 
trana. legSmot ion ( 1 J .trana. logSmoti on (2)) ; 

legt.  How  Incremental  (aquabody,  trana .  lag  (motion  10) , 
t  rana . legfimot ion  1 1 ] , t  rana . lag (mot  ion |2 ) > ; 

coord  -  Pindhoaitiona (aquabody, lagl,  lag2. lagj, lagl, lagS, lag*); 

trana  -  TranafarToGalt (coord, aquabody) ; 

break; 

H  raada  incramantal  changaa  from  a  fila 
caaa  PKEY : 

printf (”\n  heading  fila  motionin'); 
paaa  -  rila_Uaa (ifp, aquabody, lagl, lag2, lagl, 
log*, lagS, lagS) ; 

printf ("%d,  %lf,  %lt.  \lf \n", paaa .lagn urn, 

pass. doll, paaa. dol2, paaa. doll) ; 

paaa.lagnum  »  9; 

coord  -  rindhoaitiona (aquabody, lagl, Iag2,lag3, 
lag*.  lagS,  lagd  ; 

trana  -  TranafarToGalt (coord, aquabody) ; 
break; 
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rewind  (lfp)  ; 
break ; 

default: 

break: 

I  II  end  awlteh  on  event  queue  Item 
I  //  endif  qteato 


color  (IUt)i  //  background  color 

clear  () ; 

//  turn  on  Z-buffering 
(buffer  ITMIU  : 

//  clear  the  t-buffer 
(Clear  ()  ; 

bulldnonnovinqvlowlnqMtrlafvm.  vy.vr,  rfa,  rfy,  rfa)  ; 
drawaqua ( (coord. bodyc) , (coord. logic) , (coord. Ieg2c) , (coord. logic) , 
(coord. Ieg4c) , (coord. logic) , (coord . logic) ) ; 

II  turn  (-buffering  off 
(buffer  ((-ALSO  ; 

//  change  the  buffer* 
auapbufferaO : 

qenter (AKEY.l) ; 

I 

I  //  end  of  Min 


//  . . . . 

//  FUNCTION:  INITIALIZE () 

. . 

void  initialize () 


I 


//  aet  up  the  preferred  aapeet  ratio 
keepaapect (XMAXSCAEEN+l.YMAXSCREEN+l) t 

II  aet  up  window  aite 

prof poalt ion (700.0. 1200.0, 200 . 0,700.0) ; 

//  open  a  window  for  the  progran 
winopen  CAquaAobet")  ; 

//  Mke  a  title 
wintitle (“ Aqua Robot -) t 

II  put  the  IRIS  into  double  buffer  node 
doublebuffer () ; 


bot.C 
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H  eonf igure  the  IMS  IM»I  DM  the  above  eomM  Bettings) 
(CMtitOi 

//  define  ieoptikli  qu*u«i 

qdevice  ( AEDRAN) ; 

qdavlea (AKEY) ; 

qdavlea (PKfT) ; 

qdavlea  INST)  .- 

qdavlea  OKNUBUTTON)  ; 

//  initial  location  of  viewpoint  (eanera  ape) 
w  «  0.0; 
vy  *  40.0; 
n  •  400.0; 

//  initial  location  of  robot  foot  pad* 
rfx  “  0.0; 
rfy  •  0.0; 
rf*  *  -200.0; 


) 


//  . . . 

II  Function  Make  tha  Honua 

. . . . 

long  aakethenanual) 

I 


long  topmanu; 
long  caamrananu; 

//  eaaara  view* 
canaraaenu  •  newpupl); 

•ddtopup (canaraaenu, "Caaara  View  It  ■); 
addtopup (caaaraaenu, ’ABOVE  4a2  I  BEHIND  4x3”) ; 
addtopuplcaaaraaenu,  ’R10BT  SIDE  4x4  I  LEFT  SIDE  4x5’); 


//  build  tha  top  laval  menu 

t  opaanu-  dafpupt’Roll  Off  Side  It  I  Caaera  Ixl  tail 

FilaBaad  4x4  IRaaatFila  4x0 1 ReybdRead  4x7 1 
Raaat  4x14 1  Exit  4x13*, caaaraaenu) ; 

//  raturn  tha  naan  of  thia  Menu 
return (topnenul ; 


I 


II  . . . 

II  Function  Proceaa  Menu  Hit 

. . . . . 

void  proceaaaenuhit (long  hititan) 
I 

switch  thitltem) 

( 

caaa  CAMERA: 

break; 
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case  ABOVE: 

ex  •  0.0: 
ey  ”  300.0: 
ex  •  0.0; 
rfx  -  0.0; 
rfy  •  0.0; 
rfx  -  0.0; 
break; 


case  BEHIND : 

ex  -  0.0; 

vy  •  50.0; 

ex  -  250.0; 

r fx  -  0.0: 

rfy  -  30.0; 

rfx  -  -200.0; 

break; 

case  RTS IDE: 

ex  -  250.0; 
vy  -  50.0; 

ex  •  0.0; 

rfx  -  -200.0; 
rfy  -  50-0; 

rfx  «  0.0; 

break ; 

case  LTSIDE: 

ex  -  -250.0; 
ey  -  30.0; 
ex  -  0.0; 

rfx  -  0.0; 

rfy  -  0.0; 

rfx  •  -200.0; 
break; 

case  riLEREAD: 

qenter (PREY,  1) ; 
break; 

case  RESETFILE: 

qenter  <RKEY .11: 
break; 

case  RESET: 

ex  •0.0; 
vy  -  4D.0; 
vx  -  400.0; 
rfx  •  0.0; 
rfy  -  0.0; 
rfx  -  -2110; 
break ; 

case  EXIT: 

exit (01 : 
break; 

1  //  End  Switch 
I 
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//  *••*••** . . . . . 

//  FUNCTION:  BUILDNOMFWVINGVIEWINGHATRIX 

//  PURPOSE :  use  vith  objects  that  ars  in  the  iuh  coordinate 
//  system  and  aren't  moving  with  continuous 

II  rotations/translations/scalings 

//  * . * . . . * . * . . . 

void  buildnonmovingviewingmatrixlfloat  vx, float  vy, float  ex, 
float  refx, float  refy, float  raft) 

( 

loadunit  ()  ; 

pro  jectionandviewingmatrix  (vx,  vy,  wr,  refx,  refy,  refx)  ; 

1 


//  •••••••*•••»« . . . . 

II  FUNCTION:  PROJECTION  ANDVIEWINGMATRIX 

//  PURPOSE:  provides  the  projection  and  viewing  matrix 

//  * . . . . . . 

void  pro jeetionandvievingmatrix (float  vx, float  vy, float  vx, float  refx, float  refy, float  ref 
x) 

I 

II  perspective  projection  3D  for  the  world  coord  sys 
//  the  near  and  far  values  are  distances  from  the  viewer 
//  to  the  near  and  far  clipping  planes. 

//  We  are  at  (vx.vy, vx)  and  looking  towards 
II  the  center  point  of  the  object.. 

//  (towards  (refx, refy, refx) ) . 

perspective  (4S0, 1.25,  NEARCLIPPING,  PARCLIPPING)  ; 
lookat (vx, vy, vx, refx, refy, refx, 0) ; 

I 

//  . . . 

//  FUNCTION:  LOADUNIT 

II  PURPOSE:  this  routine  loads  a  unit  matrix  onto  the  top 

//  of  the  stack 

//  . . . . . . 

void  loadunit (I 
( 

static  float  un(4]|4)  -  (  1.0,  0.0,  0.0,  0.0, 

0.0.  1.0,  0.0,  0.0, 

0.0,  0.0,  1.0,  0.0, 

0.0,  0.0,  0.0,  1.0  I; 

loadmatrix (un) ; 

I 

//  •••**••••••• . . . . 

//  FUNCTION:  AQUA  DRAWING 

//  PURPOSE:  draws  the  robot  at  coordinates  provided 

. . . . 

void  drawaqua (double  'bodye,  double  *10010,  double  *leg2c,  double 
*leg3e,  double  *leg4c,  double  *legSc,  double  *legSe) 

I 

color (WHITE) ; 
linewidth (3) ; 


» 


» 


» 


» 


» 


» 


> 
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//  +x  to  right,  +y  up,  +z  out  of  teroon  ->for  graphics 
//  +x  out  of  logi,  +y  out  of  screen,  -*  down->for  kinematics 


II  *  z  y 

move (bodyc (3] , -bodyc (5) , bodyc (4) ) ; 
dr  a»  ( body  c  I S J ,  -body c  1 1  j ,  bodyc  j  7 )  >  ; 
draw (bodyc ( 9] , -bodyc Ill], bodyc [ 1 0 ) ) ; 
draw (bodyc 112], -bodyc  1 1 4  1 . bodyc [131): 
draw (bodyc [ 15] , -bodyc [ 17) ,  bodyc [ IS] ) : 
draw (bodyc [It], -bodyc (20) , bodyc [ 19] )  ; 
draw (bodyc 13), -bodyc 15), bodyc 19)); 

//  draws  a  line  from  body  csntar  to  lag!  joint  1 
linewidth (1) ; 

movo (bodyc 10) , -bodyc ( 2 ), body c [ 1 ) )  ; 
draw (bodyc [ 3 ] , -bodyc [ 5 ] , bodyc | 4 ) > ; 

//  draws  lagl 
color (YELLOW) ; 
linewidth (5) ; 

II  %  t  y 

move (logic [0] , -logic [2] , logic [ 1] )  ; 
draw (logic (3) , -logic ( 5] , logic ( 4 ) ) ; 
draw (legle IS], -logic  1*1. logic |7)) ; 
draw (logic (9) , -logic! 11 ] , logic [10] )  ; 

//  draws  log2 
color (GREEN) ; 
li nowidth (5) ; 

move ( lag2c 10), -lag2c [ 2] , log2e  1 1 ) )  ; 
draw (log2c[3) , -log2c (5) , log2e [4] ) : 
draw(leg2c(S) ,-leg2c(S),leg2c(7)) ; 
draw (log2c [ 9) , -leg2c [11 ] , log2c [ 10) ) ; 

//  draws  log3 
color (GREEN) ; 
linewidth 15) ; 

move ( log3c [0] , -log3c [2] , log3c 1 1 ) ) ; 
draw (logic [3] , -log3c (5) , log 3c [4) ) : 
draw(log3c[S], -log3c [I] , log3c [7] ) ; 
draw (leg3c [9] , -logic [11 ] ,  logic [ 10] )  ; 

II  draws  log4 
color (GREEN) ; 
linewidth  (5) ; 

move (log4c [0] , -leg4c [2] , logic  11)1; 
draw (logic  13 J , -logic (5) , logic  14) ) ; 
draw (logic [S], -logic [I], logic 1 7 ) >  ; 
draw (logic (9) , -logic [11] , logic (10) ) ; 

//  draws  log5 
color (GREEN); 
linowidth (5) ; 

move (legSc |0) , -logic [2 ) , log5c (1) ) ; 
draw ( logic [ 3 ) , -leg5c ( 5] , leg5c [ 4 ) )  ; 
draw ( logic ( S ] , -logic ( I ) , log5c 1 7) ) ; 
draw (logic ( 9) , -logic [ 11 ] , logic [ 10) ) ; 

//  draws  log6 
color (GREEN) ; 
linewidth (1) ; 

move ( logic [0) , -logSc 12) , legSc  1 1 ) ) ; 
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(tiavllfglrlJI,  -  1  *Q<T  t  ^  )  ,  lagfic  |  4  )  ) 

rlr  aw  i  l«gfc  t  () ,  -  lagCe  |  it) ,  lcgSc  !  'H  I  •' 

draw  { lag  6c  1  9) .  -Iag6e  1 1 1 ) ,  leg  6  c  1 10J ) 
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IquUf.l  1 

. . ***** .  » 


//  FILENAME :  Aqua Leg. H 

ll  PURPOSE:  Declarations  (or  AquaLeg  claaa 

II 

II  AUTHOR :  S  L  Davidson 
II  DATE:  17  Fob  <3 

//  COMMENTS :  Definition  of  AquaLeg  claaa  and  functions  that 
//  Apply  to  this  class 

//  . . . . . . . . . 

> 

fifndof  H  AOUALEG 
(define  H~AQUALEG 

(include  <atdio.h> 

(include  * Aqua robot Body .H" 

(include  *Link.H" 

(include  “LinkO.H* 

(include  “Linkl.H* 

(include  *Link2.H*  * 

(Include  *Link3.H“ 

class  Aqua Leg 
( 

public: 

//  these  dependent  objects  are  instantiated 

LinkO  *linkO;  k 

Linkl  *linkl;  9 

Link2  *link2; 

Link3  *link3; 

II  the  flag  is  set  to  1  if  the  motion  is  completed  without 
//  reaching  any  link  limits 
int  motlon_complete_f lag; 

//  the  flag  is  set  to  1  if  the  leg  is  on  the  ground  ) 

int  lag  support_f lag; 

//  the  angle  off  of  leg  one  where  the  leg  is  attachec  to 
//  the  body 

double  leg_attachaient_engle; 


Aqua Lag (AquarobotBodyC,  double);  //  constructor  and  initialiser 
-AquaLeg));  //  destructor 

void  Movelncremental (Aqua  robot  Body  (,  double  deltal,  double  delts2, 
double  delts3) ; 

double  GetLegAttschment Angle O  I  return  leg_attachment_angle ; ) 

int  GetHotionCompleteFlag ()  I  return  mot ion_complete_f Tag; I 

void  SetLegAttachment Angle {double  angle)  I leg_attachment_angle  •  angle;) 

void  SetMotionCompleteriagdnt  flag)  (motion_eowplete_f lag  •  flag;) 

int  GetLegSupportFlagO  I  return  leg_support_f  lag; ) 

void  SetLegSupportFlagdnt  flag)  |leg_support_f lag  -  flag;) 


I  : 

(endif 


» 


» 
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Aq [uaLeg.C  1 

//  ********* . . 

//  FILENAME:  Aqua Lag  .  C 

//  PURPOSE:  Implementation  of  Aqua  Lag  class 
//  CONTAINS:  Aqua Lag () 

II  Initialise (AquaLeg* ,  Aqua  robot Bodyt) 

II  TakePieture(Cax*rat,  AquaLeg* ) 

H  Moaa I ncrementa 1  (Aqualeg*,  deltal,delta2,delta3) 

II  AUTHOR:  S  L  Davidson 
//  DATE:  IT  Fab  93 

//  •*•**•« . ...a.................... . . 

♦include  "A qua Lag . H" 

//  ♦** . . . . . . . . 

//  FUNCTION:  -AquaLegO 

//  PURPOSE:  destructor  of  Aqua Leg  class 

II  ******* . 

Aqua Lag : : -AquaLeg ( t 
( 

delete  linkO; 
delete  linkl; 
delete  link2; 
delete  link3; 

I 

. . . . . . 

//  FILENAME:  AquaLeg 

II  PURPOSE:  constructor  of  AquaLeg  class 
//  RETURNS:  AquaLeg  class  with  values 

//  ......  . . . 

Aqua Lag: 'Aqua Lag  (Aqua robot Body  (body,  double  angle) 

I 

mot ion_complete_f lag  -  1;  //  initialites  flag  value 

SetLagAttachmantAngle (angle) : 

linkO  -  ne»  LinkO; 
linkl  -  new  Linkl; 
llnk2  -  new  Link2; 
link3  •  new  Link3; 


//  initial  link  values  initialized 

//  teag>  matrix  adds  in  the  T_matrix  needed  for  the  physical 
//  attachment  of  the  leg  to  the  body 
matrix  temp; 

II  updstes  the  Transformation  matrix  from  body  center  to  the 
//  leg  attachment  point 

temp.UpdateTMatrix (Get LegAtt achment Angle () ,  0  . ,  0 . ,  0 .  > ; 
temp  -  *body.H_matrix  *  temp; 

linkO->RoteteLlnk (Ctemp  , UnkO->Get Inboa rdJoint Angle () ) ; 

linkl->RotateLink (linkO->H_matrix,  linkl ->Get InboardJoint Angle O ) 
link2->RoteteLlnk (linkl->H_matrix, llnk2->Get InboardJoint Angle ( ) ) : 
link3->RotateLink(link2->H_matrix, link3->Get InboardJoint Angle (> ) ; 

//  . . * . * . . . . . 

//  FILENAME:  Movelncremental 

//  PURPOSE:  calculate  the  new  link  values  as  a  leg  rotates 
II  RETURNS:  rotated  link's  new  values  are  placed  in  the 
II  respective  leg’s  slots 

//  . * . . . 


Aqua  bar, .  C 
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void  A<j\>n!,«g  ■  :Moveincremental  (AquarobotBody  (body,  double  deltal, 
double  delta2, double  delta3) 

I 

double  b; 

//  set  all  limit  flags  to  tero 
llnkl->PetMotionLimitFlag  (0)  ; 
link2->SctMotionLimltFlag (0)  ; 
link3->SetMotionLimitFlag  (0)  ; 

//  temp  matrix  adds  in  the  T_matrix  needed  for  the  physical 
//  attachment  of  the  leg  to  the  body 
matrix  temp; 

temp .UpdateTMat rix (GetLegAttachment Angle  0 , 0 . , 0. , 0 . ) ; 
temp  -  ‘body . H_matrix  *  temp; 

linkO->RotateLink ((temp, linkO->Get InboardJoint Angle () ) ; 

b  -  deltal  4  linkl->GetInboardJointAngle () ; 
linkl->SetlnboardJointAngle (b) ; 

linkl~>RotateLink (linkO->H_mat rix, linkl->GetInboardJuintAngle  () ) 

b  -  delta2  +  linfc2->GetInboardJointAngle () ; 
link2->SetlnboardJointAngle (b) ; 

link2->Rotate (linkl->H_matrix,  link2->GetInboardJointAngle () ) ; 

b  -  delta3  4  Hnk3->GetInboardJointAngle () ; 
link3->SetlnboardJointAngle (b) ; 

link3->RotateLink  (link2->l!_matrix,  link3->Get InboardJointAngle ( )) 

II  the  mot ion_complete_f lag  is  set  to  1  if  the 
//  motion_limit_f lags  on  all  legs  are  not  set 

SetMot ionCompleteFlag ( ! <linkl->GetHotionLimitFlag{)  I  I 

1 ink2->GetMot ionLimitFlag ()  II  link3~>GetMot ionLimitFlag () ) ) ; 

//  prlnta  the  status  of  the  requested  motion  and  prints  which 
//  link's  mot ion_limlt_f lag  was  set  (if  any), 
if  (GetMot ionCompleteFlag (1  —  0) 

(printf ("Motion  Not  Completed\n") ; 
if  (1 ink l~>Get Mot ionLimitFlag ( )  —  1) 
printf  ("link  1  limit  exceeded\n") ; 
if  (link2->GetMotionLimltriag()  —  1) 
printf  ("link  2  limit  exceeded\n") ; 
if  (1 ink3->GetMotionLimitFlag 0  —  1) 
print f  ("link  3  limit  exceededNn") ; 

I 

else  printf ("Mot  ion  completedVn") ; 


I 
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//  . . . . . . . 

U  FILENAME :  Aqua robot Body.M 

//  MIRPOtt:  Dacia ration  of  AquarobotBody  claaa 

//  Sabclaas  of  RlqidBody  claaa 

//  AUTHOR:  S  L  Da. Ida  on 

//  DATE:  20  Sap  S2 

//  . . . 

flfndaf  H  AOUAAOEOTEODT 
fdafina  H_AQUA*ODOTBCOT 

fincluda  <atdio.h> 
fincluda  "ftigldBody .H" 
fincluda  "MatriaMy .H" 

claaa  AquarobotBody  :  public  RlqidBody 
( 

public: 

■atria  *body_liat;  //  dafinaa  tha  aiaa  of  tha  body  oainq  coordinataa 
doubla  asinuth,  alavatlon,  roll: 

AquarobotBody O ;  //  eonatructor 

aoid  Moaalncraaantal (doubla. doubla,  doubla,  doubla,  doubla,  doubla); 


I; 

fandif 
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//  . . ****** . . 

U  FILENAME  AquarobotBody .C 

//  PURPOSE:  laplaaentation  of  the  AquarobotBody  claa* 

//  CONTAINS :  initialises  tha  body  fen 
//  AUTHOR:  S  L  Davidson 
//  OATS:  17  Nov  S3 

//  .*•••*•• . . . ••••••*••*•••* 

•include  " AquarobotBody. B* 

//  . . . . . 

//  FUNCTION:  AquarobotBody  O 

//  PURPOSE :  eonatrueter  of  tha  AquarobotBody  elaaa 
//  RETURNS:  Aquarobotledy  elaaa  with  waluaa 
//  . . . . . . . 

Aqua robot Body: : Aqua robot  Body () 

I 

//  each  row  la  a  body  point  (a,y,a) 

//  tha  firat  (0  row)  la  tha  body* a  phyaieal  cantor,  and  tha  raat 
//  aru  tha  eia  points  of  tha  body 
body_liat  m  now  aatrla(7, 4, 0.0) ; 

//  tha  body's  coordlnatoa  ara  daflnad  eantarad  at  0,0,0 
body_liet->val(0,S)  -  0.:  body  _1 1st -wa  1(0, 1)  »  0.: 
body^liat-Pwal (0, 2)  «  0.;  body~list->val (0, 3)  «  1.: 
body_llat->waUl,0)  •  37.5:  body_list-»val (1, 1)  •  0.; 
body  list->val (1,2)  -  0.;  body  llst->val (1, 3)  *  l.r 
body_llat-»wal (2, 0)  -  IS. 75:  body_list->vsl (2, 1)  -  32.41; 
body  list->val (2,2)  •  0.;  body  list->val (2, 3)  •  1.; 
body_llot-»wal (3,0)  -  -11.75;  body_list->val (3. 1)  -  32. 4S; 
body_liat->val<3,2)  •  0.;  body_llst->val (3, 3)  -  1.; 
body_llat->wal(S,0)  «  -37,5;  body_liat->val (4, 1)  -  0.; 
body- llet-»vel (4,2)  -  0.;  body  llst->val (4, 3)  -  1.; 
body_liot->wal(3.0)  -  -IS. 75;  body_list->*al (5, 1)  -  -32. 4S; 
body_liat-»val (5,2)  -  0.:  body  list->val(5,3>  -  l.» 
body  Ust->walU,0>  -  IS. 75;  body  list-Aval (S,  1)  -  -32.41; 
body_liet->vel ((, 2)  -  0.;  body_li#t->»al (S, 3)  -  1.; 

//  defines  tha  Initial  location  of  tha  body  usinq  the 
//  H-mntria  tha  inputs  to  tha  function  are: 

//  (aaiaaith,  elevation,  roll,  a,  y,  a) 

Hjaatria-PHoaoqanaousTransf on(0 .  ,0 . ,0. , 0.  ,0. ,  -S4.1B19) ; 

//  Novas  tha  body  coordinator  to  tha  initial  location  daalrod 
body_liat->Trenefonal.iot  (*H_«atria,  *bedy_llst) ; 

) 

. . . . . . . . . 

//  function :  MovalncroaMntal 

//  PURPOSE:  tha  body  ia  aovad  based  upon  cowandad  incranntal 
//  daqreaa  of  ehanqa  that  arc  passed  in 

. . . . . . . 

void  AquarobotBody: :  Move  Inc  reawital (double  dalat,  double  delel, 
double  delrol,  double  dels, double  daly,  double  dels) 

I 

double  at,  ol,  ro,  a,  y,  s; 
at  -  asianith  *  dalat; 
el  -  elevation  *  delel; 
ro  -  roll  ♦  delrol; 
a  •  body_liat->vsl(0,0)  4  dela; 
y  -  body~liat->val(0,l)  4  daly; 
t  •  body_list->vel(0,2)  4  dolt; 
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Vj"  arobotPody .  C 


a 


//  only  ehangat  in  uaad  bolov  a  Inca  body_liat  Is  at  currant  position 
H_matrla->Hoa»panaou8Trana(ora(dalax,  dalal,  dalrol,  dolx,  daly,  dolt); 
body_llst->Tranafon«Llst  I'RjUtrli,  *body_liat> ; 

//  puts  all  Info  In  H_aatrla 

H_aatrlx->lloaiog«n*ouaTransfoza(aa>  al.  to,  a,  y ,  a)  > 


ities.C 


1 


//  riLCNAHC:  Kinwtlet.C 

II  WWO$t!  to  dtttmlM  position*  (*,y,  t)  fro*  tho  H_*atrla 
//  :  to  road  fro*  a  flla  the  now  link  arfU  chanyta 

II  and  update  th*  apprepiat*  leg/llnk  values 

//  t  to  paaa  ite*e  to  tho  gait  function 

//  AUTBOO:  S  L  Daoldaon 
//  DATE:  IS  February  1113 


fincludo  <*ath.h> 

•include  <atdlib.h> 
fincludo  <atdio.h> 
fincludo  "HatriaMy.il" 
fincludo  "AquaLog.H" 
fincludo  "AquarobotBody 
fincludo  "Link.K" 

fdofino  GROUNDELEVATION  0.0 

//  structure  doalgnod  te  receive  fii*  input 
atruct  Passlng_ltens  I 
int  lognu*; 
int  body; 
doublo  doll; 
double  do 12; 
doublo  d*13; 
doublo  dalf; 
doublo  dalS; 
double  dalC; 

I; 

II  structure  roeiovas  noat  desired  robot  notion  fro*  gait 
//  planning  functions 
struct  Noat  Notion  ( 


II  desirod  joint  increment  values  roturaod  fro*  th*  gait  function 
doublo  body*otion|C] ; 
doublo  l*gl*otion|3); 
doublo  lag2mot ion 1 3 ]  ; 
doublo  l*g3*otion(3] ; 
doublo  lagfnotion ( 3] ; 
doublo  lagSnot ion ( 3] ; 
doublo  logfnot ion ( 3  J ; 

//  actual  position  status  values  eont  to  gait  function 
double  l*g_contact_f lag  If); 
doublo  jolnt_li*it_flaglll] ; 
doublo  foot_l_coordl3j ; 
doublo  f  oot~2_coord 13); 
double  foot~3_coord(3) ; 
doublo  f oct~f~coord 13); 
doublo  f oot~S_eoord 1 3} ; 
double  foot~f_eoord(3) ; 
double  body~c*nt*r_eoordlf); 


//  structure  designed  to  consolidate  the  ay*  coordinates  of  the  robot 
atruct  Ketum_Coordinatea  I 
double  bodyc|21); 
double  loglcl 12); 


I 


double  leg2c|12) ; 
double  l<fje|12]; 
double  lef4e|12|; 
double  leg5c(12); 
double  letter 12); 
lot  s»tion_lieiit_flagtH) 
lnt  leg_aupport_f  lagK) ; 


//  xyr  coordinate*  are  detenelned  free  the  R_»atrix  end 

//  return  the  Cartesian  coordinates  using  the  Retum_Coordinatas  structure 

Return  Coordinates  FindFositions (Aquarobotbody  sbody,  AquaLeg  (legl. 

Aqua Leg  (leg 2, AquaLeg  (leg a,  AquaLeg  (lag*,  AquaLeg  (legs, 

AquaLeg  SlagO 

( 

Return_Coordinates  *rc; 
re  -  new  hetum_Coordinates; 

//  body  center  coordinates 
rc->bodyc(0)  “  body .body_list->»*l (0,0) ; 
rc->bodye(l)  -  body.body~liat->*al(0,l) ; 
rc->bodyc(2)  -  body ,body_liat->*al (0,2) : 

II  body  points  to  drau 
rc->bodyc(3]  -  body .body_liat->eal Cl, 0) ; 
rc->bodyc(4)  “  body ,body_list->*al (1,1) ; 
rc->bodye(S)  -  body .body_list->»al (1, 2) ; 
re->bodyc(<]  “  body. body_list->eal (2,0) ; 
re->bodyc(7)  •  body .body_list->eal (2, 1) ; 
rc->bodyc[*J  -  body. body~list->*al (2,2) ; 
re->bodyc(»)  “  body .body~liat->*al (3, D) i 
rc->bodyc (10)  -  body .body_liat->»al (3, 1) : 
rc->bodye(ll)  *  body.body_list-»*al(3,2)  i 
rc->bodyc(12]  «  body ,body_list-»*al (4, 0) : 
re->bodyc(13]  -  body ,body_liat->»al (4, 1) ; 
rc->bodye ( 1 4 ]  «  body .body_list->»al (4,2) ; 
re->bodyc(15)  •  body. body_ll*t->»al (5,0) ; 
rc->bodyc{lC]  -  body ,body_l 1st ->»al (5, 1) ; 
rc->bodyetl7)  -  body .body_list->*al (5,2) : 
rc->bodyc(ll)  -  body .body_list->»el (*,  0)  ; 
rc->bodyc[l»]  »  body.body_list->»el ((,  1) ; 
rc->bodyc|20]  •  body.body_list->*el((,2) ; 

//  prints  out  body  coordinates 

printf ("body  center  43f  ,  »3f  ,  »3f  \n", rc->bodyc(01 , re->bodyc (1) , 
rc->bodyc(2)) ; 

printf ('body  pt  1  %3f,  »3f,  %3f  \n", rc->bodye(3), rc->bodyc(4) , 

rc->bodycl5)) ; 

printf ("body  pt  2  »3f,  %3f,  »3f  \n", re->bodyc(*J , re->bodyc ( 7 I , 
rc->bodyc (II ) ; 

printf (“body  pt  3  »3f,  »3f,  43f  \n*, rc->bodyc [* 1 , re->bodyc 110) , 
rc->bodyc|ll] ) : 

printf ("body  pt  4  43f,  %3f,  43f  \n", rc->bodyc (12) , re->bodyc ( 131 , 
rc->bodyc|14)) ; 

printf ("body  pt  5  %3f,  »3f,  %3t  \n-, rc->bodyc( 15) , re->bodyc(l*) , 
rc->bodyc ( 17 J ) ; 

printf ("body  pt  (  I3f,  %3f,  %3f  \n\n",rc->bodyc(l§),rc->bodyc(l*J, 
rc->bodyc(20] ) j 


//  Joint  one  leg  coordinates:  (0)**  [lj«y  I2J-* 
rc->leglc(0)  -  legl . llnk0->H_matri*->**l (0, 3)  ; 
rc->leg2c|0)  ■  leg2 .linkO->H_aatrix->val (0,3) ; 
rc->leg3e(0j  «  leg3.1inkO->H_*iatri»->**l(0,3) ; 
re->leg4cl0)  •  leg4  ,llnk0->H_a»strl*->*sl  (0, 3) : 


itloa  .C 


I 


rc->lt(Se[0) 
re->lagtclO) 
rc->laglc(l] 
rc->lag2c(l j 
rc->lag3c[lj 
rc->lag4c(l j 
rc->l*gSc(l) 
re->l*gfc(l] 


1({S .  llnkO->H_a»tria->*al  (4,3); 
lag(.link4->R~a*tria->*al(4,3) > 
lagl ■ linkO->M~aat  ria-x*a 1 (1, 3) ; 
lag2.11nk4->Rjaatria->aal(l,3)  ; 
lag3. Ilnk4->R  aatrla->aal(l,3) ; 
lag4 . linkO->R~aiatrla->»il  <1,  3* ; 
lagS.  linkO->R_iaatria->val  (1, 3) ; 
l*g(.link4->Rjaatrla->aal  (1,3)  i 


re->laglc(2) 

rc->lag2c(2) 

rc->lag3c[2) 

rc->lag4cl2] 

rc->lag5e|2J 

rc->lagfc(2) 


lagl .  Iink4->R  antrla->*al  (2,3); 
lag2. llnkl->H~aatrla->val (2, 3)  ; 
lag3 . linkl->H_«at ria->aal (2,3); 
Iag4.1inkl->R  aatrix-yval (2, 3) ; 
l*gS.llnkl->Rj**tria->*tl{2,3) ; 
lagt.linkl->H_*atrix->val (2,3) ; 


//  joint  2  a, y, 
re->l*glct3)  ■ 
rc->lag2c[3)  • 
rc->lag3e(3)  “ 
rc->lag4e[3)  - 
rc->lag5e|3)  ■ 
re~>lag«c(3)  - 
rc->l*glc(4)  - 
rc->lag2c|4)  - 
re->laglc(4)  - 
rc->lag4cl4]  ■ 
rc->lagSc(4)  • 
re->lagtc(4)  • 


X  coordlnataa  1 3]  “a  (4]-y  |5J-x 
lagl.llakl->H  a»tria->»al(0,3) ; 
lag2 .  linkl->H_*at rla->»al  (4, 3)  ; 
lag3.1inkl->H_aatrla->*al(0,3) ; 
l«f4 . llnkl->R_aatri*->*al (4,3); 
lag5 . linkl->H~aatrin->*al (4, 3) ; 
lag<.link)->H  aatrix->val  (4,3)  ; 
la4)l .  linkl->Hj*atria->*al  (1,3); 
lag2 . linkl->R_aatrla->val (1,3); 
lag3 . 1  inkl-»T*»t  ria->val (1,3); 
1*4)4 .  linkl~>R~mtria->*al  (1,3)  ; 
Iag5.1inkl->R  aatria->*al(l,3) ; 
lagf . llnkl->B~*atria->T*l (1, 3)  ; 


rc->lagle[S) 

rc~>lag2c(SJ 

rc~>lag3e(S] 

rc->lag4c[S] 

rc->l*gSc|5] 

re»l*gCc(S) 


lag) .  linkl->R_mtrla->val  (2, 3)  ; 
l*g2 . linkl->H_*atria->*al (2, 3)  ; 
lags .  linkl->R_Mt  rla->val  (2, 3)  ; 
lag4 .  linkl->Hjaatria->val  (2,3); 

lags .  llnkl*>R  *atri*->»al (2, 3) ; 

lagt . llnkl->Hjaatria->val (2,3) ; 


//  joint  iaotion_llaUt_f lag 

rc->»otlon_liadt_f  lagTO)  -lagl .  llnkl-MSatMotionLimltriag |) ; 
rc->iaot  ion_l  1*1  t_f  lag  1 3 )  -  l*g2  .llnkl-XMtMotionLlailtriagO  ; 
rc->aK>tion_liiait_flag(<]  -  lag3 .  linkl -XJatMot  ionLimltriag  ( )  ; 
rc->iaotion_ll*it_flag(>)  -  log 4.  linkl-kGatHotlonLiatitriag  ()  ; 
tc->motion_li*it_f lagliu  -  lags . llnkl-XiotNotlonLlatltrlagO 
rc->aiotlon~lli«lt_f lag [15)  -  lag 6 . linkl-kGatMotlonLlaltPlagO 


//  joint  3  ay* 
re->laglc(«]  - 
rc->lag2c(S]  - 
rc->lag3c[«J  - 
re->l*g4c(()  - 
rc->lagSe[4)  - 
rc->l4»g4ci*)  - 


coordlnataa  (*)-a  [7)-y  (*)-t 
lagl .  Unk2->H_*atrla->val (4, 3)  i 
lag2 . link2->Hjaatria->*al (4, 3) ; 
lag3  .  Unk2->H_iaatrix->val  (4, 3)  ; 
lag4 .  link2->R_aiatrlx->*al  (4,3); 
lags . link2->H_*atria->»al (4, 3) ; 
lag4.1ink2->R_iaatxlx->val(4, 3) ; 


re->lagle[7) 

rc->lag2c|7] 

re->lag3c[7) 

rc->lag4e[7) 

rc->lagSc[7J 

rc->lag*c|7) 


lagl .  Iink2->n_natrla->*al  (1,3) ; 
lag2.1ink2->R_aiatria*>«al(l,3) ; 
lag3.11nk2->H_aiatrln->aal(l,3) ; 
lag4 .  llnk2->R~aiatrlx->*al  (1, 3) ; 
lags.  link2->H_aiatria->aal(l,  3) ; 
lagl.  llnk2->M_aiatria->aal  (1,31 ; 


rc->laglc[4) 
rc->lag2e|4) 
re -> lag 3  c  1 9 ] 
rc-»lag4c|8) 


lagl .llnk2->Hj*atria->*al (2,3) ; 
lag2 .  link2->H_Mt  rla->aal  (2, 3) ; 
lag3. link2->H_awtrla->«al (2, 3) ; 
l*g4.1ink2->R_Mtria->Tall2,3)  ; 
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rc->  Sell]  -  lagS.link2->n_iaatrix->val(2,3); 
rc->lagic(t]  -  1«(< .  llakl*>ll'ittrif>«il  <1 , ))  I 

//  joint  3  notion_lladt_fl*g 

rc->*otion_limit_flag(lT  •  lagl .  llnk2->QatMotionLiaitriag()  i 
rc-Xwt  lag  1 4)  •  lag2.1ink2-><9*tMotionLialtFl*g(); 

rc->f»ot  ion_li*it_f  lag (7 ]  •  lag3.1ink2-XJatMotionLi>itFlagO,- 
re->«ot ion” li*it~f lagtlOJ  -  log4.1ink2->GatMoeionLiakltFlagO  t 
re->t*ot  ion_li»it~flagtl3}  •  lagS.link2->OatMoelonLiatitriag()  > 
rc->*«otio«i~lij*lt~f  lag  (14  j  •  lagC.link2->OotMotioaLladeriag(l  i 

//  joint  4  lyi  eoordinataa  |41»i  (10)-y  (111”* 
rc->lagle(f]  •  lagl .llnk3->H_Mtric->**l (0, 31  ; 

*e->lag2e|»l  -  lag2.1ink3->M~a»tria->*al (0, 3) t 
rc->lagjc(t]  »  lag3.1ink3->Hjaatrla->val  (0,3) ; 
rc->lag4e(»)  •  l«g 4 . Iiak3*>ll'aatria-»al (t, II ; 
re->lag5e(#j  *  lagS.llak3->H_i*atri*->val<0,3)  ; 
rc->lag4c(fj  •  lagf  .link3->n~aiatri*->aal(0,3) ; 

rc->l*glc(10J  •  lagl.  Iink3->ll_aatrlr->*al  (1,3)  : 
rc->lag2c(10]  •  l*g2 .link3->H_jaatria->val (1, 3) ; 
re->lag3e(101  •  lagl .  link3->B_«a*tri*->»al  (1, 3) ; 
rc->l#g4e (10J  •  lag4.1ink3->H_*»tri*->»al (1, 3) ; 
re->lagSe|10)  »  lag5.1ink3>>H_aiatrix->val  (1, 3) ; 
rc->lag4c(10]  •  lagi.link3->lt~a*tzlx->valu,31 s 

rc->lagle(lll  -  lagl.link3->H_aatria->*al 12, 3) ; 
re->log2c(ll)  •  lag2.1ink3->H_aiatrix->Tal (2, 31  s 
re->lag3e(lll  •  lag3.1ink3->H_matrix->val  (2,3)  ; 
rc->lag4e(llj  -  lag4 .  link3->H_*»atrix->»sl <2, 3) : 
ze->lagSef 11}  •  lag5.1ink3->M_aatria->*al(2, 3) » 
rc->lag4c(ll}  ”  lagt .link3->H_a«tri*->aal (2, 3) ; 

//  joint  3  awt  ion_li»it_f  lag 

rc->i*otion_liadt_flag(21  «  lagl .UnkS-XlatHotionLlialtFlagO  ; 
rc->«ot ion_limit~flag(5]  -  log2 .  UnkS-MJatMotlonLiadtFlag  ()  ; 
rc->motlon_liadt_flagll}  -  l#g3.1ink3->G*tMotionLi*dtriag  ()  ; 
rc->*otlon_liadt~flag[ll]  -  lag4 . link3->GatMot ionLialtriag <> ; 
rc->«otion_liatit_fXag(14]  -  Xag5.11nk3->GatltotionLiialtFlagO  ; 
cc->antion_liBit_fXag(11)  •  lagi.linkS-XSatMotionLiiaitFlagO  ; 

//  toot  for  aupporting  log*  and  adjusting  lagaupport  flag 

if  (tabs  (rc->logle(ll]l  >-  OHOWDeLCVATlOHI  lagl .SatLagSupportFlag tl)  t 
ala*  lagl . SatLagSupportFlag (0) ; 

if  (faba(rc->lag2c(ll])  >-  OKOUHDEUEVATION)  lag2. Sat LagSapportFlag <1> ; 
ala*  lag2.SatLagSupportFlag(0> : 

It  (f aba ( re->lag3c (XU)  >-  GMOWDCLCVATXON)  lag3.SatLagSupportriag(l)  ; 
alaa  lag3. SatLagSupportFlag (01 i 

if  ( f aba ( rc->lag4c (111)  >-  SROONOELKVATION)  lag4 . SatLagSupportFlag (1) ; 
alaa  lag4 .SatLagSupportFlaglO) ; 

if  (faba(rc->l*g$c[lll)  >-  GXOONDCLCVATION)  lags. SatLagSupportFlag (1)  : 
alaa  lag! . SatLagSupportFlag (0) i 

if  (tab* (re*>lagtc I 111 1  >•  OHOOWdelevatioh)  lagi. Sat LagSapportFlag (1) ; 
alaa  lag(. Sat LagSapportFlag (01 : 

//  placaa  lag_aupport_f lag  into  re 

rc->lag_*upport_f laglO)  •  lagl  .OatLagSupportrlagO  ; 
rc->log_#upport~flag(ll  •  lag2 .GotLagSopportFlagO ; 
rc-»lag_aupport”f lag (2 J  «  l*g3.G«tLagSupportFlag<) 
re->lag_*upport_f lag|3)  *  lag4.GatLagSupportFlag() ; 
rc-»lag~»upport”flag 1 4 1  •  lags .OatLagSupportFlag () ; 
re->lag~aupport~flag(S]  *  lagf  .OatLagSupportrlagO  ; 

//  print*  body  and  lag  ayr  eoordinataa 


fin— tie*. C  5 

int  row,  col; 

prlntf  ("lagl  log2\n*); 

for  (  row  •  1;  row<S: 

( 

for  <col  -  3;  col>0;  col — ) 

prlntf (■**. 4f  ", rc->laglc[3  •  row  -  col)); 
prlntf I*  *) ; 
for  (col  -  3;  col>0;  col--) 

prlntf ("16. 4f  ", re->lag2c [3  *  row  -  col]); 
prlntf  ("\n") ; 

I 

prlntf ("\n"( i 

prlntf ("lag3  lag4\n"); 

for  (row  -  l;  row<5;  row++) 

( 

for  (col  -  3tcol>0;  col — ) 

prlntf ("16. 4f  ", re->lag3c(3  *  row  -  col)); 
prlntf (“  *1 ; 

for  (  col  •  3;  eol>0;  col--) 

prlntf ("16. 4f  ”, rc->lag4c (3  *  row  -  cel)); 

prlntf  ("\n") : 

) 

prlntf  ("\n") ; 

prlntf ("lag5  lastin') ; 

for  (row  -  1;  row<5;  row++) 

I 

for  (col  -  3;  eol>0;  col — ) 
prlntf (*t(.4f  *, rc->lagSc(3  *  row  -  col]); 

prlntf (*  *); 

for  (col  «  3;  eol>0:  col — ) 
prlntf (*»6.4f  ", re->lag6e  (3  *  row  -  col)); 

prlntf ("\n") ; 

I 

prlntf ("\n") ; 
rat urn  *re; 

1 

//  . . 

//  FUNCTION:  Fila_U»a 

//  PURPOSl :  raada  desired  lac  changes  from  •  file 
//  INPUT:  raada  from  flla: 

//  format:  legl ,  daltal.  delta2,  delta3 

//  OUTPUT:  calculator  naw  leg/llnk  coordinataa 

//  . . . . . . . . . . . 

Passing_Items  Flle_uaa (FILE  * If p, Aqua  robot Body  tbody, AquaLeq  tleql, 
AquaLeg  4leg2, AquaLeg  61ag3,  Aqua Lag  41eg4,  AquaLeq  tlegS, 
AquaLag  tlagf) 

( 

Paaalng_Itama  *paaa; 
paaa  -  naw  Passing  Items; 

facanf (ifp, "Id  llf""llf  Uf  Ilf  Ilf  llf-,6pass'>body,*pese->dell, 
tpaaa->dal2, tpasa->dol3, 4paaa->dal4, 4pass->delS,*pass->del6) ; 
if  (pass->legnum  <  9) 

I 

body .Howe Incramentsl <pees->dell,  pass ->dal2,pass->dal3,paas->dal 4, 
pasa->dalS,pass->dal<) ; 

fscanf (ifp, "Id  tlf  tlf  Ilf",  tpsss->legnum, (pass->dell, tpass->del2, 
tpass->dal3) ; 

lagl .Movalncramantal (body, pass ->dall, pass- >dal2,  pass- >del 3) ; 
fscanf (ifp, "Id  Ilf  Ilf  Ilf ", 4pass->lagnum, Ipass-XJell, 6pass->dal2, 
lpaas->del3) ; 
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« 


leg2 .Movelne tenant al  (body,pase->dell,  paaa->del2,  paaa->del3)  ; 
faeanf (ifp, *»d  U(  %lf  31f",*peas->legnun, cpaas->dell,lpaea->d%12, 
4paaa->del3) ; 

1*93 .Movelnereaaental (body,  para ->d«ll,  pae«->del2, paaa->del3> ; 
faeanf  dtp,  *%d  %lt  %lf  3lf’,  lpess->legmim,  tpaas-xlell.  tpaes~>del2, 
tpasa->del3) : 

leg4 .Hove Incremental (body,pass->dell, paas~>del2,pess->del3) ; 
faeanf (ifp, "%d  %lf  »lf  %lf*,tpaaa->legnua, tpaas->dell,4pass->del2, 
tpase->del3> ; 

lafS.Nmlncranatal  (body,pasa->dell,paas->del2,pses~>del3)  ; 
faeanf (ifp. *%d  Uf  tlf  31f*,ipaaa->legnua, lpaaa->dell, lpeas->del2, 
lpas«->del3) ; 

leg! .Movelncrenental (body,pass~>dell,paae~>del2,paas->del3) ; 
faeanf  (ifp,*  3d  %lf  %lf  Ilf  ",  lpasa->legntm,  tpaas-xiell,  tpass->del2, 
4pess->del3) ; 
it  (paas->legnum  —  0) 

I  pa  a  a - >de 1 1  -  0.0; 
pasa->del2  -  0.0; 
paaa->d«13  -  0.0; 

)  ; 
t; 

return  • pa  a  a ; 

) 

//  *********************************** . 

//  FILENAME :  Transf erToGait 

//  FtjAPOSE:  placaa  the  body  center  and  leg  Carteaian  coordinates 
//  In  a  Neat  Motion  atructura  for  gait  algorithm  use 

//  ****** . «»*•••** . . . 

Next  Motion  TranaferToGait (Return  Coordinates  acoord.  A qua robot body  ibodyl 

( 

Next_Motion  *taag>; 
teiap  -  new  Hext_Motlon; 


temp->body_eenter_eoord|3)  «  coord. bodyc  10) ;  Hr 
temp-rbody^center^coordll]  -  coord .  bodyc 11 ) ;  //y 
tenp->body^center_eoord(S)  »  coord. bodyc 12) ;  //r 


teiap->foot_l_coord|0]  - 
te*g>-»foot_l_coord(l]  « 
temp->f ootlcoord 12)  - 

temp->foot_2_coord(0 J  «* 
tenp->foot_2_coord|l)  «■ 
teap->f oot_2^eoord (21  - 

teiap-»foot_3_coord(0)  - 
te*p->foot_3”eoord(l j  - 
teiap->foot_3_coord |2)  * 

tenp->foot_4_eoord|0)  • 
tewp->foot_4_eoord[l)  - 
teng>-»foot_4~coord(2)  • 

teep*»foot_i_eoord|0)  • 
tenp->foot_i~eoOrd(li  • 
temp->foot_5_coord(2 J  • 

temp->foot_€_eoordl0)  • 
tenp->foot_l_eoord(l ]  • 
teirp->foot~6”coord(2)  • 

H  current  body  elevation 


coord. logic (9) ; 

/  /  X 

coord ■ leglc (10) ; 

Hy 

c oord. logic ( 11 ] ; 

//* 

coord. Ieg2c(»); 

//X 

coord. Ieg2c (10) ; 

//y 

coord. Ieg2ef 11); 

Hr 

coord. Ieg3c (91 ; 

Hr. 

coord. logic  |10) ; 

H  y 

coord. logic j 11); 

Hr 

coord. Ieg4c(9] ; 

Hr 

coord . Ieg4c | 10] ; 

H  y 

coord. logic  |11) ; 

Hr 

coord. logic ( 9) ; 

Hr 

coord.legleflO); 

"V 

coord. leglc |11); 

Hr 

coord. leglc ( 9 ] ; 

Hr 

coord. logic | 10); 

Hy 

coord. logic  ill) ; 

Hr 

Mm—  tle«.C  7 

temp->body_eenter_coord{ll  «  -1.  *  body .H_matrix->val (2, 0) ; 
//  currant  body  ariauth 

te*p->body_center_eoord(01  -asin(body.K_matrix->val(l,0)  / 
eoa  (teag>->body_center_coord  1 1 1 )  >  ; 

II  currant  body  roll 

temp->body_canter_coerd(2)-asin (body .H_matrix->vel (2,1)  / 
coa (tesp->body_center_eoordll)7) ; 

//  Joint_limit_f lag 
for  (int  i-0;  1<17;  i++) 

temp-> joint_li*it_flag(i)  -  coord. motion_li»»it_flag(i]; 

//  leg_contlct_flag 
for  (int  j«0;  J<7;  j++) 

temp->leg_contact_flag( J)  -  coord.  leg_support_f  lag  1 1) 
return  *temp; 

) 

. . . . . . . . . . 

//  FUNCTION : gait  algorithm 

//  PURPOSE:  to  provide  a  temporary  gait  function  for 
//  teating  purposes 

//  * . * . . . . . 

Next_Motion  GaitAlgorithm  (Next_Motion  tin) 

( 

NextHotion  'temp: 
tang:  -  new  Next_Hotion; 

for  (int  1  •  0;  i<6;  i++) 

temp->bodyiaot  ion  I  i  ]  -  0.0; 

for  (i  -  0;  i<3;  i+.) 

(  temp->leglmotion(i)  -  0.0; 
temp->leg2motion(i)  •  0.0; 
temp->leg3motion(ij  «  0.0; 
teag>->leg4motion(ij  -  0.0; 
te«g>->leg5motionIi)  -  0.0; 
temp->legfimotion(i]  -  0.0; 

I; 


//  movement  desired 

return  ‘temp; 

I 


» 


I 


» 


I 


I 


» 
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//  . . . 

//  FILENAME:  Link. II 

//  PURPOSE:  Declarations  for  elaas  Link 

// 

//  AUTHOR:  S  L  Davidson 
//  DATE:  IS  Sapt  »2 

//  COMMENTS:  Definition  of  Link  class 
//  .......................................................... 

tlfndef  H  LINK 
* define  h~LINK 

(include  <atdio.h> 

(include  <*iath.h> 

(include  "RlgidBody . H* 

(include  "MatriaMy .H" 

class  Link:  public  RigidBody 
( 

private: 

int  motion_lii«it_f  lag: 

double  linklength; 

double  tviet_angle; 

double  inboa rd_joint_angle; 

double  inboard_ joint_displaceawnt ; 

double  inboard_link; 

double  min_joint_angle;  //  rotary  link 

double  *ax_}oint_angle;  //  rotary  link 

public: 

Link  (  int  mlf,  double  11,  double  ta,  double  ija,  double  ijd,  double  11, 
double  xiin_Ja,  double  awx_)a  I: 

-Link (I ; 

void  Rotate (aiatrla*,  double) ; 
void  RotateLink (matrix*,  double); 

int  GetMotionLimitriagO  (return  i*otion_limit_fIag;  I 

double  GetLinkLength ()  (return  link_length; ) 

double  Get Tvist Angle ()  I  return  tvist_angle; ) 

double  GetlnboardJointAngleO  (return  inboard_ joint_angle; I 

double  GetlnboardjointDisplaeeieent  <)  (return  Inboa rd_jolnt_displace*ent; ) 

double  GetlnboardLink ()  I  return  inboard_llnk; ) 

double  GetMinJointAngleO  (return  siin_joint_angle; ) 

double  Get Max Joint Angle 0  (return  x»x_)oint_angle; I 

void  SetMotionLimitFlagdnt  a)  («otion^llisit_f lag  •  a;) 

void  SetLinkLength (double  a)  (llnk_length  -  a;) 

void  SetTwist Angle (double  a)  !tvist_angle  -  a; I 

void  SetlnboardJointAngle (double  a)  ( inboard_Joint_angle  -  a; I 

void  Set InboardJointDisplaceeent (double  a)  I  inboa rd_joint_diaplacement  -a;) 

void  SetlnboardLink (double  a)  ( inboard_link  -  a;) 

void  SetMlnJointAngle (double  a)  (min_ jolnt_angle  -  a;) 

void  SetMaxJointAngle (double  a)  |ma*_ joint_angle  -  a;) 


); 

(endif 
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. . .  ................. 

//  FILENAME:  Link.C 

II  PURPOSE:  Implementation  of  claps  Link 
//  CONTAINS:  UpdateAMatrlx  O 
//  Rotate  (double  angle) 

//  RotateLink  (double  angle) 

//  AUTHOR:  S  L  Davidson 

//  DATE:  ItSept  92 

//  * . ............ . . 


linclude  "Link .H" 

const  int  True  *1; 
const  int  False  -  0; 


//  . . . . . . . 

//  FUNCTION:  Link 
//  PURPOSE:  Constructor  for  Link 
II  RETURNS:  a  link  with  valuas 
//  . . . . . 

Link:  .-Link  (  int  mlf,  double  11.  double  ta,  double  ija,  double  ijd,  double  11. 
double  lain  Ja,  double  an  Ja  ) 

1 

motion_limlt_f lag  -  mlf; 
link_length  •  11; 
twist_angle  -  ta; 
inboerd_Joint_engla  •  ija; 
inboard'joint_diaplacemont  -  ijd; 
inboard_llnk  -  il; 
min_jolnt_angle  -  min_ja; 
max_Jolnt_angle  *  max) a : 

H  mtrix->UpdateTMatrlx(i  Ja,  ta, 11, ijd) : 

) 

//  * . * . . . . . . . . 

II  FUNCTION:  -Link 

//  PURPOSE:  destructor  for  Link  class 
II  . . * . . . . . . 

Link: : -Link () 

I 

delete  node_llst; 

I 

//  . . 

II  FUNCTION:  Rotate 

//  purpose:  rotates  a  Link  by  changing  the  T  Matrix 
//  by  the  inboard  joint  angle  desired 

//  RETURNS:  an  updated  T  matrix  within  the  Link  object 

//  . . . . . 

void  Link::Rotste  (matrix  ‘mat,  double  angle) 

I 

Sat  Inboard Joint Angle (angle) ; 

T_matrix->UpdateTMat rix (Get InboardJo int Angle () , Get Twist Angle () , 

GetLinkLength (I .GetlnboardJointDlsplacement () ) ; 
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//  the  "met"  la  the  Inboard  link's  T  tutrix  (or  th«  body's 
//  T  matrix  for  tha  Inboard  joint 
•H  matrix  »  *mat  •  *T  matrix; 


//  . . . . . . . 

//  FUNCTION:  RotataLlnk 

//  PURPOSE:  datanalnas  If  tha  rotation  la  within  physical 
//  joint  eonstrainta.  If  outaida  tha  workspace  the  min 
//  or  aax  limit  applicable  la  used. 

//  :  this  function  calla  tha  Rotate  function 

//  RETURNS:  sets  range  of  Inboard  joint  angle  If  desired  is 
//  outside  physical  constraints 

// 

void  Link: : RotataLlnk (matrix  “mat,  double  angle) 

I 

double  tester:  //  temporary  variable 

tester  -  GetMlnJolnt Angle () j 
if  (angle  <  tester) 

(  angle  -  tester; 

SetHotionLimitFlag(l)  ; 

) 

tester  »  GetMax Joint Angle (; ; 
if  (angle  >  tester) 

I  angle  -  tester; 

SetNotionLimitPlag(l) ; 

I 


Rotate (mat,  angle); 


LinkO  .  n 


1 


//  . . . . . 

//  F  1  LF.HAMF,  ■  LinkO.H 

//  FUfU'fiSF :  Declarations  for  class  LinkO 
// 

//  AUTHOR:  S  L  Davidson 
//  DATF.:  17  Sept  92 
/ /  COMMENTS : 

//  . . 

♦ i f ndef  HLINKO 

♦  define  H_MNK0 

♦  Include  "Link.ll” 

class  LinkO  :  public  Link 

I 

private : 
puhl ic : 

LinkO  {)  ; 

)  ; 

♦ondif 
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n  . . . 

U  riLBMMB:  LinkO. C 

//  PURPOSE:  Daclarationa  for  elaaa  LinkO 

// 

//  AUTHOR:  S  L  Davidaen 
//  DATE:  17  tape  *2 
U  COMCNTS: 

1 1  • . . . * . . 

tincloda  "LinkO. ■’ 

LinkO : : LinkO O  s  Link  (  0,  37.3,  0.0,  0.0,  0.0,  -1.0, 
-310. 0,300.0) 

( 

noda_liat->aal(0,3)  -1.;  noda_llae->*al (1, 3)  »1.» 
noda~liat->val (2, 0)  >37.3;  noda_llot->»al (2, 3)  •  1 . ; 


II  . . . . . 

II  riLCHMC:  Linkl. R 

//  PURPOSE:  Declaration*  for  elaa*  LinkO 

II 

II  AUTHOR:  S  L  Da.idaon 
//  DATE:  17  Sopt  *2 

//  concurs: 

//  . . ..., 

sifndof  h  Lima 
•define  H~Lima 

•  include  "Link  .  R* 

claaa  Linkl  :  public  Link 
( 

private: 

public: 

Linkl () t 

}; 

tendlf 


u  . . . 

1 1  PILdMAME:  Llnkl.C 

//  PURPOSE:  Declarations  for  class  LinkO 

// 

1 1  AUTHOR:  S  L  Davidson 
U  DATE:  1?  Sept  02 
1 1  COWdUTI: 

1 1  . ************* . * . ********************* 

(include  "Linkl.H* 

Linkl: :Linkl(>  :  Link  (  0,  20.0,  -00.0,  SS.4,  0.0,  0,~10(.C,73.4) 

I 

nede_list  -  new  aatris(4, «, 0.0) ; 
noda_liat->val (0,  3)  -1.; 
node_list->val (1, 3)  “1 .< 
node~li*t->val (2,0)  -  20.0; 
noda_list->val (2, 3)  •  1.; 


T  Matrix  •  now  aiatris  (4,  4, 0.0) ; 


//  . . . 

II  FILENAME:  Link2.ll 

//  PURPOSE :  Declaration*  for  claaa  LinkO 

II 

II  AUTHOR:  S  L  Davidaon 
II  DATE:  17  Sapt  12 
//  COHMMTS: 

II  ...................................... 

tifndef  H  LINK2 
I define  HLIHK2 

f include  *Link.H” 

claaa  Link2  :  public  Link 
( 

private . 
public: 

Link2()  ; 

)l 

tendif 


uatt.e 


i 


> 


» 


//  . . . 

//  FILENAME:  Link2.C 

//  PURPOSE:  Declarations  for  elaaa  LinkO 

// 

//  AUTHOR:  S  L  Dari da on 
//  DATE:  IT  Sept  *2 

//  C0M4ENTS:  . 

H  . .  » 

f include  "Link2.ll* 

Link2 : : Llnk2 { )  :  Link  (  0,  50.0,  0.0,  -ISC. 4,  0.0,  1.0,  -15C.4,  23.4) 

< 

node_liat  -  now  atttria (4, 4,0.0) ; 
node_liat->»al(0,3)  -1.;  node_liet->*al (1, 3)  “1 .  ; 

node_list->val (2, 0)  -50 . ;  node_liat->val (2, 3)  -  1.;  | 

T_Mtrir  -  now  laatrix  (4,  i,  0 . 0) : 


I 


I 


» 


» 


» 


» 


I 


► 
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//  riLEMAME:  Link3 .  H 

//  purpose:  Declaration*  for  claa*  LinkO 

//  AUTHOR:  S  L  Davidson 
U  DATE:  17  Sept  »2 
//  CCWENTS: 

. . * . a . 


•ifndaf  H  LINKS 

*  define  iTlinks 
(include  -Link.H* 
class  Links  :  public  Link 


private: 

public: 

Llnk3  O ; 
I; 

lendif 


I 


I 
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//  . . . . 

//  riLENKME:  Link 3. C 

//  rainst:  Declaration*  for  elaaa  LinkO 

// 

//  AUTHOR:  S  L  Da. Ida on 
//  DATS:  17  Sopt  >2 
//  COHMWTS: 

//  . . . . . . . 

•include  ‘Links. H- 

Link):: LinkS Cl  :  Link  (  0,  100.0,  0.0, 0.0,  0.0,  2.0,  •3(0.0,300.0) 

( 

node_liet  •  now  natriuM,  4, 0 .0)  ; 

node_liat~>.al (0, 3)  -l.j  noda_liat->val 11, 3)  -1.; 

node~liet->»el (2, 0)  -100. j  node_ll*t->»al (2,  3)  -  l.i 

T  Matrix  -  now  mat rix (4, 4, 0 .01  > 
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//  . . . 

//  FILENAME :  MatriiiMy.il 

//  PURPOSE:  To  provide  for  a  Matrix  claaa  to  accomplish 
//  som  nacassary  robotic  and  kinematic  naads. 

//  AUTHOR:  S  L  Davidson 
//  DATE:  29  Oct  92 

//  COtMEMTS :  DHMatrlx,  Homogeneous  Transform,  and 
//  TransformList  ara  inclodad 
//  . . . . . 

tifndaf  H  MATRIX 
Idafina  H~MATRIX 

const  doubla  deg_to_rad  ■  .011453292519913295; 
class  aiatrix 
( 

struct  rnatrap 

I 

double  **m; 
int  r,  c,  n; 
l*p; 

public : 

suitrixlconst  matrix*  x) : 

-matrix!) ; 
matrix!) ; 

matrix!int,  int,  double  ) ; 
matrix  operators Iconet  Mtrixt  real); 
matrix  operator-* Iconst  matrix*  real) ; 
matrix  operator* Iconst  matrix*  rval) ; 
matrix  operator* (double) ; 
double  *  vallint  row,  int  eoDeonst; 
void  print  |); 

int  rows!)  const  Ireturn  p->r;l; 
int  cola!)  const  (return  p-»c;l; 

//  Craig  method  used 
matrix  *  HomogeneousTransform (double, double, double, double, doubla, double) 
matrix  t  DHMatrlx  Idouble,  double,  double,  double,  double,  doubla); 
matrix  t  UpdateTMatrix (doubla,  doubla,  double,  double); 
matrix  t  TransformList (matrix*,  matrix*); 


//  copy  initialiser 
//  class  destructor 
//  class  constructor 
II  class  constructor 


II  spot  value 
tt  prints  matrix 

II  returns  number  of  rows 
//  returns  number  of  columns 


tendif 


Rat  flatly . C 
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//  . . . . 

//  FILENAME :  MatrixMy.C 

//  rimror.F :  Implementation  of  MatrixMy  class 
//  CONTAINS-  functions  which  operate  upon  matrix 
/ /  type  variables 

//  AUTHOR:  S  I,  Pavidson 
//  PATE:  20  Feb  93 

n  . . * . * . • 

♦include  <;tdio.h> 

J include  <stdlib.h> 

♦include  <strinq.h> 

♦include  <math.h> 

♦include  "MatrixMy. H" 

♦  include  "AquaLeg.H" 

II  . * . . . . 

II  FUNCTION:  matrix!) 

II  PURPOSE.:  constructor  of  a  matrix  type 
II  :  creates  a  4  by  4  matrix  (by  default) 

II  RETURNS:  s  matrix  with  0.0  in  all  spaces 

//  . . . . 

matrix: : ma  t  r 1 x ( ) 

I 

p  -  new  mat  rep; 
p->r  ~  4; 
p  -  >  c  -  4  ; 

p->m  -  new  double  *[4); 
int  x; 

for  (x  ”0 ;  x<4 ;  x44 ) 

p->m|x)  “  new  double [41]; 

p->n  «  1 ; 

int  j ; 

for  (int  i-0;  1<4;  1+  +  ) 
for  (j-0;  j<4 ;  j  +  4) 
p->m ( i ) ( j)  -  0.0; 

I 

II  . . . 

//  FUNCTION:  matrlxlrow,  col,  initval) 

II  PURPOSE:  constructor  of  a  matrix  type 

II  :  creates  a  1  by  1  matrix  by  default  in  which  all  the 

II  item  values  are  0.0  or  matrix  sixe  and  values 

//  indicated 

//  RETURNS:  a  matrix  of  sltedasired  with  initial  values  desired 
//  . . 

mat r 1 x : :mat r 1 x ( int  rows  ”  1,  int 

I 

p  -  new  mat  rep;  :  / / 

p->r  -  rows:  II 

p->c  *»  col;  II 

p->m  -  new  double  •(rows);  // 

II 

i  r,  t  x  ; 

for  In  ~0;  x<rowS:  x*») 

p/->m|x)  *  new  double(col);  II  each  row  is  given  an  array  equal 

II  to  the  number  of  columns  desired 

p-  .«n  -  J; 


col  -  1,  double  initvel  •  0.) 

pointer  to  matrix  structure 
r  is  number  of  rows 
c  is  number  of  columns 
produces  the  desired  number 
of  rows 


II  pointer  to  mstrix  structure 

//  r  is  number  of  rows 

II  c  is  number  of  columns 

//  m  is  the  value  array 

II  m  consists  of  a  4  pointer  array 


//  produces  an  array  of  four 
//  items  per  array  pointer 


II  each  matrix  is  given  the  initial 
//  valus  of  0.0 
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int  j; 

for  (int  i-0:  Krowa;  i++) 
for  (j-0;  }<col;  j++) 

p-xsdl  Ij)  -  initval;  //  initialixoa  oach  value  to 
//  da si rad  initval 

I 

//  . . . . . 

//  FUNCTION:  matrix  (matrix) 

//  PURPOSE:  daap  copy  conatructer  of  the  matrix  type 
//  RETURNS:  a  coeplete  identical  copy  of  the  matrix 
. . . . . 

matrix: (matrix (const  matrix*  x) 

( 

x.p->n++; 

P  “  x.p; 

I 

//  . . . . . . . . . . 

//  FUNCTION:  operator- 

//  PURPOSE:  operator  overload  function  of  the  equals  sign 
//  produces  another  matrix  which  points  to  tha  original 

//  returns:  copy  of  matrix  is  made  in  the  other  one 
//  . * . . . * . . . . 

matrix  matrix:  (operator- (const  mstrixt  rval) 

( 

if  ( — p->n  --  0) 

( 

for  (int  x-0;  x<rows();  x++) 
delete  p->m(xl ; 
delete  p->m; 
delete  p; 

) 

rval.p->n++; 

P  -  rval.p; 
return  *this; 

1 

//  . . . . . . 

//  FUNCTION:  -matrix () 

//  PURPOSE:  destructor  of  a  matrix  type 

1 1  . * . . . . . 

matrix: (-matrix () 

( 

it  ( — p->n  —  01 
I 

for  (int  x-0;  x<rows();  x++)  delete  p->m[x): 
delete  p-x»; 
delete  p; 

I 

I 

//  . . . . . . * . * . . 

//  FUNCTION:  val'row,  col) 

II  PURPOSE:  finds  the  value  in  a  matrix  given  row  and  column 
//  RETURNS:  value  in  spot  in  desired  row  end  column 
//  . . . 

double  t  matrix: :val (int  row,  int  col)  const 


a 

€ 

4 


i 


» 


» 


»  « 


> 


> 
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3 


I 

return  <p->m|row) |col)) ; 

) 

//  eeeeeeeeeeeeee . . 

//  FUNCTION:  operator* 

II  PURPOSE :  oparator  overload 

//  :  provides  Multiplication  of  tvo  matrices 

//  RETURNS:  the  product  of  tvo  matrices 

II  . . . . . 

matrix  matrix:  :oparator*  (conat  matrix!  arg) 

I 

matrix  raault  (rowa  O  .  arg  .cola  ()  ,0 .0)  .•  II  tamporary  matrix  eonatructad 

for  (int  row-0;  rowCrows  () ;  row-**) 

(  int  col; 

for  (eol-0;  coKarg.eola  (> ;  col++) 

I 

double  oum-0.0; 

for  (int  i-0;  i<colsO;  144) 

sum  4-  p->m[rov] |i]  *  arg. val ti, col) ; 
result. val (row, col)  -  sum; 

I 

I 

return  raault; 

I 

. . * . . . . . . 

//  FUNCTION :  operator* (double) 

//  PURPOSE:  operator  overload 

//  provides  multiplication  of  a  scalar  and  a  matrix 

II  RETURNS:  tha  matrix  product 

//  . . 

matrix  matrix: :operator* Idouble  a) 

( 

matrix  result (rowa () .cola (), 0 .0) ;  II  temporary  matrix  constructed 

for  (int  i-0;  i<rows();  i4*) 

( 

for  (int  j-0;  j<cols();  }+♦) 

(  double  ana; 

ana  -  result .val (i, j)  *  a; 
result .val (i, j)  -  ana; 

) 

I 

return  result ; 

I 

II  * . * . . . . 

II  FUNCTION:  operator. 

II  PURPOSE:  operator  ovarload 
//  provides  addition  of  two  matrices 

//  RETURNS:  the  matrix  sum 

1 1  . . * . . . . 

matrix  matrix:  :operator4(eonst  aiatrix!  arg) 

( 

matrix  sumlrowa I) , cols () ,  0. 0) ;  //  temporary  matrix  constructed 

for  (int  i-0;  i<rows();  144) 

(  int  j; 

for  (j-0;  j<cols();  j+4) 
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4 


[  J)  -  p->n(l)tj)  ♦  a rg . va 1 ( i ,  j  >  ; 

) 

mturn  sun; 

I 

n  . . . . . . .  '< 

!!  FUNCTION:  print  0 

//  PURPOSE:  prints  the  values  of  the  matrlr, 

//  RETURNS:  a  print  out  to  the  screen  of  the  matrix  contents 
//  ******** . . . . . * . . 

void  mat r lx : :prlnt ( ) 

I 


for  (lnt  row“0;  rov<rowa();  row**) 

I 

int  col; 

for  (col-0;  col<eola{);  col++) 

pr int f ( "46 . 6f  ",  p->m(row) (col ) ) ; 
printf ("\n”) ; 

1 


//  . * . * . * . * . * . * . 

//  FUNCTION :  Homogeneous  Transform 

//  PUPfOSE:  constructs  a  transformation  matrix 

//  RETURNS:  a  matrix 

//  . . . . . 4 . 44*4.44444 . 4.4 

matrix  l  mat rlx :: HomogeneousTransf orm (double  arimuth, double  elevation, 
double  roll,  double  x, double  y,  double  r) 

I 

double  spsl  -  sin (arimuth) ; 
double  cpni  -  cos (arimuth) ; 
double  ath  «*  sin  (elevation)  ; 
double  cth  -  cos (elevat ion) ; 
double  sphi  -  sin(roll); 
double  cphi  -  cos (roll); 


val (0,0) 

(cpsl  •  cth); 

val (0, 1! 

( (cps l  *  ath  * 

aphl)  - 

(spsi 

*  cphi ) ) 

val (0,2) 

( (cpsi  •  sth  • 

cphi)  ♦ 

(spsl 

*  sphi)) 

val  (0, 3) 

x; 

val (1, 0) 

(apsi  *  cth); 

val (1,  1) 

( (cpa 1  •  cphi) 

♦  (spai 

*  sth 

*  sphi)) 

val  (1,2) 

((spai  •  ath  • 

cphi)  - 

Icpsi 

•  sphi)) 

val  (1,3) 

y; 

val  (2,0) 

(-sth) ; 

val  (2,1) 

(cth  *  sphi) ; 

val  (2,2) 

(cth  *  cphi); 

val  (2,3) 

r  > 

val  (3,  0) 

0.0; 

val (3,  1) 

0.0; 

val (3, 2) 

0.0; 

val (3. 3) 

1.0; 

return  *  t  h  i  a ; 


I 

//  . . . * . . 

II  FUNCTION :  OH  Matrix 

II  PURPOSE:  constructs  a  Dll  suitrix 
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//  RETURNS:  a  matrix 

H  . . 


matrix  t  mat.  rlx :  :DKMatrix  (double  cosrotate.  double  sinrotate, 
double  costwiat,  double  aintwist,  double  length, 
double  tranalate) 


val (0, 0) 
val (0,1) 
val (0, 2) 
val (0, 3) 
val (1, 0) 
val  ( 1 , 1) 
val  (1,2) 
val (1, 3) 
val (2,0) 
va 1 (2 , 1) 
val (2, 2) 
val (2,3) 
va  1 (3, 3) 


cosrotate; 

-1  *  sinrotate; 

0.0; 

length; 

sinrotate  *  costwiat; 
costwiat  *  cosrotate; 

-1  *  aintwist; 
tranalate  *  -1  *  aintwist; 
aintwist  *  sinrotate; 
aintwist  *  cosrotate; 
coatwist; 

tranalate  *  costwiat; 

1.0; 


return  ‘this; 

1 


//  . . . . . . . . 

//  FUNCTION:  Update  T  Matrix 

//  PURPOSE:  constructs  a  transformation  matrix 
//  ;  calls  the  DH  matrix  function 

//  RETURNS:  a  matrix 

//  MM.*..*..... . . . . . . 

matrix  t  mat r lx : :UpdateTMatrix (double  rotate_angle,  double  twiat_angle, 
double  length,  double  translation) 

I 

rotate_angle  -  rotate_angle  *  deg_to_rad; 
tvlat_angle  -  twiat_ang.le  *  deg_to_rad; 
double  cosrotate  -  cos (rotate_angle) ; 
double  sinrotate  «  sin(rotate_angle) ; 
double  costwiat  »  cos (t*»ist_angle) ; 
double  aintviat  -  a  in (t wiat_angle) ; 

I 

DHMatrix (cosrotate,  sinrotate,  costwist,  aintwist, length,  translation) ; 

return  ‘this; 

I 

//  . . . . . * . . 

//  FUNCTION:  Transform  List 

//  PURPOSE:  transfers  coordinates  to  new  position  baaed  upon  H_matrix 
//  RETURNS:  a  transformed  node  list  as  a  matrix 
//  . . . . . 

matrix  l  matrix : :Transf ormLiat  (matrix  *H  matrix,  matrix  Sb) 

I 

matrix  temp(4, 1,0.0) ;  //  temporary  matrix  constructed 

for  (int  i  “  0;  i<b.rows();  i*+) 

( 

//  transposes  the  node_llst  so  multiplication  can  be  accomplished 
temp . va 1 (0, 0)  -  b. val (1,0); 
temp, val (1, 0)  -  b.val (1, 1) ; 
temp. val (2, 0)  -  b.val(l,2); 
temp. val (3, 0)  »  h. veld, 3); 
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matrix  middle  -  H_rrjtrix  *  temp; 

//  transposes  the  node_liat  back  to  original  form 
val(i,0)  “  middle .val (0,  0) ; 
val(i,l)  -  middle. val (1,0)  ; 
val(i,2)  -  middle .val  (2, 0) ; 
val(i,3)  -  middle . val (3, OS ; 

I; 

return  “this; 

I 
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1 


//  riLENAME:  RigidBody.H 
//  PURPOSE:  construct  tM  »up*Pel»«» 
//  AUTHOR:  S  L  Dl»id*On 
//  DATE:  1*  S*pt  *J 
//  . . ************ 


tot  robot  »y»t*»a 


tlfndaf  H_RIGIOBOOY 

Id* fin*  HJUSIOBOOr 

I inclod*  -MAtrlAMy.H* 

const  doubl*  gra»ity  -  32.21*5; 


class  RigidBody 

( 


public: 

matrix  *no<le_list; 

Matrix  *H_*atrix  ,  ‘Tjaatrix; 

RigidBody () ; 

-RigidBody ( > ; 


I: 

Isndif 
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1 


//  * . 

//  FILENAHE:  RigidBody. C 

//  PURPOSE:  Iapl«Hnt«tion  of  class  RigidBody 
//  CONTAINS:  superclass  of  robot  spates: 

//  :  coemon  slots  initiated 

//  AUTHOR:  S  L  Davidson 
//  DATE:  IS  Feb  S3 
// 

finclude  "RigidBody .H" 

//  . . 

//  FUNCTION:  RlgidBodyt) 

//  PURPOSE:  constructor  of  Rigid  Body  class 
//  RETURNS:  produced  Rigid  Body  class 
//  *•*••** . . . 

RigidBody: :RigidBody() 

( 

node_liat  -  nev  isatris  H,  4,  0 .0)  ; 

H  sstrix  •  nev  Matrix (4,  i,  0 .0) : 

T_siatris  *  new  matrix (4, 4, 0 .01 : 
l:- 


//  .••**.»••»••••* . . . 

//  FUNCTION:  -RigidBody () 

//  PURPOSE:  destructor  of  the  class 

//  . . * . . 

RigidBody : : -RigidBody I ) 

I 

//  delate  node_list: 

//  delete  H_*atrix; 

I 
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APPENDIX  C  -  CLOS  SCRIPT  AND  GRAPHICS 


typescript  1 

Script  started  on  Wed  Mar  10  08:35:48  1993 
hydra%  cl 

Allegro  CL  4.1  [SPARC;  Rl|  (7/8/92  9:07) 

;;  Copyright  Fran*  Inc.,  Berkeley,  CA,  USA 

; ;  Unpublished.  All  rights  reserved  under  the  copyright  laws 
;;  of  the  United  States. 

;;  Restricted  Rights  Legend 


;;  Use,  duplication,  and  disclosure  by  the  Government  are  subject  to 
restrictions  of  Restricted  Rights  for  Commercial  Software  developed 
;;  at  private  expense  as  specified  in  DOD  FAR  52.227-7013  (c)  (1)  (ii) . 

;;  Optimization  settings:  safety  1,  space  1,  speed  1,  debug  2 

?;  For  a  complete  description  of  all  compiler  switches  given  the  current 

;;  optimization  settings  evaluate  (EXPLAIN-COMPILER-SETTINGS) . 

USER(l) :  (load  "load-files. cl") 

;  Loading  /n/aquarius/work/mcghee/aquarobot/load-f i les . cl . 

;  Loading  /n/aquarius/work/mcghee/aquarobot/camera . cl . 

;  Loading  /n/aquar lus/work/mcghee/aquarobot /I ink . cl . 

;  Loading  /n/aquarlus/work/mcghee/aqua tobot /rigid-body . cl . 

;  Loadi ng  /n/aquarlus/work/mcghee/aquarobot/robot-kinematics . cl . 

;  I.oading  /n/aquarius/work/mcghee/aquarobot/aqua . cl . 

.*  Loading  /n/aquarius/work/mcghee/aquarobot/aqua-leg.cl . 

;  Loading  /n/aquarius/work/mcghee/aquarobot/aqua-link . cl . 

T 

l»SF.R(2):  (aqua-picture) 

Nil. 

USER  (3)  :  (setf  move-list  '(<0  0  0  0  0  0)  (0  0  0)  90  0  0)  (.1  .2  .3)  (0  0  0) 
(000)  (000))) 

((0  0  0  0  0  0)  (0  0  0)  (0  0  0)  (0.1  0.2  0.3)  (0  0  l ;  (0  0  0)  (0  0  0)) 

USF.RM):  (move-incremental  aqua-1  move-list) 

T 

USER (5) :  (new-picture) 

NIL 

USER (6):  (exit) 

;  killing  "Default  Window  Stream  Event  Handler" 

;  killing  "Xll  event  dispatcher" 

;  killing  "Initial  Lisp  Listener" 

;  Exiting  Lisp 
hydtak  exit 
hydra* 

script  done  on  Wed  Mar  10  08:50:16  1993 
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